#include "rads_business.hpp"
std::mutex g_db_mutex;
ros::Publisher p;
ros::Publisher pcron;
ros::Publisher pcronapi;
ros::Publisher papp;
ros::Publisher Rradar;
// 变道自动驾驶
void radarThreadFunction_R(std::vector<car> pathPoints, std::vector<car> pathPointsB, int loop_count = 1, int autocron_id = 0)
{
    // 状态机定义（新增B线计数状态）
    enum LaneState
    {
        NORMAL,             // 正常路线行驶
        LANE_B,             // 路线B行驶
        LANE_B_COUNTER,     // 路线B计数（需跑2个点）
        CHANGING_TO_B,      // 变道至路线B
        CHANGING_TO_NORMAL, // 从路线B返回正常路线
        WAITING_LEFT,       // 正常路线等待左侧障碍
        WAITING_RIGHT       // 路线B等待右侧障碍
    };
    LaneState current_state = NORMAL;

    // 新增：B线计数变量（记录已跑的B线点数）
    int lane_b_point_counter = 0;
    const int LANE_B_REQUIRED_POINTS = 1; // B线需跑2个点

    // PID控制器初始化
    EnhancedAnglePIDController enhanced_pid(
        CONFIG_KP, CONFIG_KI, CONFIG_KD,
        CONFIG_MIN_OUTPUT, CONFIG_MAX_OUTPUT,
        stop_car, Stop_lidar);

    // 控制变量初始化
    double distance;         // 当前点位距离
    double NDT_JDC;          // 原始角度
    double NDT_JDC_new;      // 修正后角度
    int speed = MINSPEED;    // 当前速度
    int maxspeed = MAXSPEED; // 最大速度
    int minspeed = MINSPEED; // 最小速度
    int speed_new;           // 修正后速度
    int api_change_counter_update = 0;
    int speed_change_counter_update = 0;
    int current_loop = 0; // 当前圈数

    bool waiting_left_timeout = false;                             // 左侧等待超时标记
    std::chrono::steady_clock::time_point waiting_left_start_time; // 等待开始时间
    const int WAITING_LEFT_TIMEOUT_SECONDS = 60;                   // 超时阈值（1分钟）

    // 路线索引管理
    int snapped_index = 0;     // 正常路线当前索引
    int snapped_index_b = 0;   // 路线B当前索引
    bool need_re_snap = false; // 是否需要重新吸附

    // 辅助函数：计算车辆到路径点的距离
    auto distance_to_point = [](const car &point)
    {
        if (lidar_rtk.matching_rate > 45.0)
        {
            return distance_count(lidar_rtk.x, lidar_rtk.y, point.x, point.y);
        }
        else
        {
            return distance_count(lidar_rtk.rtk_x, lidar_rtk.rtk_y, point.rtk_x, point.rtk_y);
        }
    };

    // 主循环
    while (current_loop < loop_count && !stopRadarThread_R.load())
    {
        // 默认正常路线吸附点
        snapped_index = findSnappedWaypoint(pathPoints);
        if (snapped_index != -1)
        {
            const car &target_pointxf = pathPoints[snapped_index];
            if (lidar_rtk.matching_rate > 45.0)
            {
                std::cout << "当前吸附到点位: [" << target_pointxf.num_point << "] (" << target_pointxf.x << ", " << target_pointxf.y << ")" << std::endl;
            }
            else
            {
                std::cout << "当前吸附到点位: [" << target_pointxf.num_point << "] (" << target_pointxf.rtk_x << ", " << target_pointxf.rtk_y << ")" << std::endl;
            }
        }
        else
        {
            std::cout << "未找到有效吸附点" << std::endl;
            stopRadarThread_R.store(true);
        }

        // 内层循环：单圈行驶逻辑
        while (snapped_index < pathPoints.size() && snapped_index_b < pathPointsB.size() && !stopRadarThread_R.load() && (lidar_rtk.matching_rate > 45 || lidar_rtk.posi_flag >= 4))
        {
            api_change_counter_update++;
            speed_change_counter_update++;

            // 路线切换时重新吸附
            if (need_re_snap)
            {
                if (current_state == NORMAL || current_state == CHANGING_TO_NORMAL || current_state == WAITING_LEFT)
                {
                    snapped_index = findSnappedWaypoint(pathPoints);
                    snapped_index = snapped_index + 2;
                    std::cout << "重新吸附到正常路线点 [" << snapped_index << "]" << std::endl;
                }
                else if (current_state == LANE_B || current_state == LANE_B_COUNTER || current_state == CHANGING_TO_B || current_state == WAITING_RIGHT)
                {
                    snapped_index_b = findSnappedWaypoint(pathPointsB);
                    snapped_index_b = snapped_index_b + 2;
                    std::cout << "重新吸附到路线B点 [" << snapped_index_b << "]" << std::endl;
                }
                need_re_snap = false;
            }

            // 输出当前点位信息及单双车道状态
            const car &target_point = (current_state == NORMAL || current_state == CHANGING_TO_NORMAL || current_state == WAITING_LEFT)
                                          ? pathPoints[snapped_index]
                                          : pathPointsB[snapped_index_b];
            std::cout << "当前目标点: [" << target_point.num_point << "] "
                      << "距离: " << std::fixed << std::setprecision(2) << distance_to_point(target_point) << "米 | "
                      << "车道类型: " << (target_point.ext == 1 ? "双车道" : "单车道")
                      << std::endl;

            // 计算到当前目标点的距离
            distance = distance_to_point(target_point);

            // 到达点位：基于距离推进索引
            if (distance < DRIVE_DISRANCE)
            {
                std::cout << "已到达点位 [" << target_point.num_point << "]，距离: " << distance << "米" << std::endl;

                // 推进当前路线索引
                if (current_state == NORMAL || current_state == CHANGING_TO_NORMAL || current_state == WAITING_LEFT)
                {
                    snapped_index++;
                }
                else
                {
                    snapped_index_b++;

                    // B线计数逻辑：仅在计数状态下累加
                    if (current_state == LANE_B_COUNTER)
                    {
                        lane_b_point_counter++;
                        std::cout << "路线B已行驶 " << lane_b_point_counter << "/" << LANE_B_REQUIRED_POINTS << " 个点" << std::endl;
                    }
                }

                // 检查是否完成当前路线
                if ((current_state == NORMAL || current_state == CHANGING_TO_NORMAL || current_state == WAITING_LEFT) && snapped_index >= pathPoints.size())
                {
                    std::cout << "完成正常路线所有点位" << std::endl;
                    break;
                }
                if ((current_state == LANE_B || current_state == LANE_B_COUNTER || current_state == CHANGING_TO_B || current_state == WAITING_RIGHT) && snapped_index_b >= pathPointsB.size())
                {
                    std::cout << "完成路线B所有点位" << std::endl;
                    break;
                }

                // 继续处理新点位
                const car &next_point = (current_state == NORMAL || current_state == CHANGING_TO_NORMAL || current_state == WAITING_LEFT)
                                            ? pathPoints[snapped_index]
                                            : pathPointsB[snapped_index_b];
                std::cout << "移动至下一点 [" << next_point.num_point << "]，距离: "
                          << distance_to_point(next_point) << "米" << std::endl;
                continue;
            }

            // 核心新增：在路线B行驶时，检测正常路线当前点的stop_flag和stop_time
            if (current_state == LANE_B || current_state == LANE_B_COUNTER || current_state == CHANGING_TO_B || current_state == WAITING_RIGHT)
            {
                if (snapped_index < pathPoints.size())
                {
                    const car &normal_route_point = pathPoints[snapped_index];

                    if (normal_route_point.stop_flag == 1)
                    {
                        std::cout << "路线B行驶中，检测到正常路线当前点 [" << normal_route_point.num_point
                                  << "] 触发交互，等待处理..." << std::endl;
                        pubdrive(100, 2, 18000, 0);
                        usleep(20000);
                        continue;
                    }

                    if (normal_route_point.stop_time > 0)
                    {
                        std::cout << "路线B行驶中，检测到正常路线当前点 [" << normal_route_point.num_point
                                  << "] 需要等待 " << normal_route_point.stop_time << " 秒" << std::endl;
                        pubdrive(100, 2, 18000, 0);
                        sleep(normal_route_point.stop_time);
                        continue;
                    }
                }
            }

            // 正常路线stop处理
            if (current_state == NORMAL || current_state == CHANGING_TO_NORMAL || current_state == WAITING_LEFT)
            {
                if (target_point.stop_flag == 1)
                {
                    std::cout << "点位 [" << target_point.num_point << "] 触发交互，等待处理..." << std::endl;
                    pubdrive(100, 2, 18000, 0);
                    usleep(20000);
                    continue;
                }
                if (target_point.stop_time > 0)
                {
                    std::cout << "点位 [" << target_point.num_point << "] 需要等待 " << target_point.stop_time << " 秒" << std::endl;
                    pubdrive(100, 2, 18000, 0);
                    sleep(target_point.stop_time);
                    snapped_index++;
                }
            }

            // 发布进度
            if (api_change_counter_update >= API_CHANGE_INTERVAL)
            {
                api_change_counter_update = 0;
                publishSradsackProgress(
                    (current_state == NORMAL || current_state == CHANGING_TO_NORMAL || current_state == WAITING_LEFT)
                        ? snapped_index
                        : snapped_index_b,
                    current_state == NORMAL || current_state == CHANGING_TO_NORMAL || current_state == WAITING_LEFT
                        ? pathPoints
                        : pathPointsB,
                    speed_new,
                    target_point.line_id,
                    target_point.map_id);
            }

            // 计算角度
            if (PID_STATUS == 0)
            {
                NDT_JDC = (lidar_rtk.matching_rate > 45.0)
                              ? calculate_NDT_JDC(target_point)
                              : rtkcalculate_NDT_JDC(target_point);
            }
            else
            {
                double raw_angle = (lidar_rtk.matching_rate > 45.0)
                                       ? calculate_NDT_JDC(target_point)
                                       : rtkcalculate_NDT_JDC(target_point);
                NDT_JDC = enhanced_pid.calculate(0.0, raw_angle);
            }

            // 正常驾驶逻辑（核心修改：添加单/双车道判断）
            if (stop_car == 1)
            {
                // 单/双车道通用逻辑：仅检测前方障碍（Stop_lidar）
                if (Stop_lidar == 1)
                {
                    std::cout << "前方有障碍，停车等待" << std::endl;
                    pubdrive(100, 2, 18000, 0);
                    continue;
                }

                // 核心逻辑：根据目标点ext判断单双车道，仅双车道启用变道逻辑
                bool is_double_lane = (target_point.ext == 1); // ext==1为双车道，ext==0为单车道

                // 双车道逻辑（启用变道）
                if (is_double_lane)
                {
                    // 障碍检测与变道逻辑（正常路线保持原逻辑）
                    // if (current_state == NORMAL || current_state == WAITING_LEFT) {
                    //     if (Stop_lidar0 == 1) {  // 前方有障碍（仅双车道检测）
                    //         bool left_blocked = (Stop_lidar5 == 1 || Stop_lidar6 == 1 || Stop_lidar7 == 1);
                    //         if (left_blocked) {
                    //             std::cout << "双车道-前方+左侧有障碍，停车等待" << std::endl;
                    //             pubdrive(100, 2, 18000, 0);
                    //             current_state = WAITING_LEFT;
                    //             continue;
                    //         } else {
                    //             std::cout << "双车道-前方有障碍，变道至路线B" << std::endl;
                    //             current_state = CHANGING_TO_B;
                    //             need_re_snap = true;
                    //         }
                    //     }
                    // }
                    if (current_state == NORMAL || current_state == WAITING_LEFT)
                    {
                        if (Stop_lidar0 == 1)
                        { // 前方有障碍（仅双车道检测）
                            bool left_blocked = (Stop_lidar5 == 1 || Stop_lidar6 == 1 || Stop_lidar7 == 1);
                            if (left_blocked)
                            {
                                std::cout << "双车道-前方+左侧有障碍，停车等待,Stop_lidar5:" << Stop_lidar5 << ",Stop_lidar6:" << Stop_lidar6 << ",Stop_lidar7:" << Stop_lidar7 << std::endl;
                                pubdrive(100, 2, 18000, 0);

                                // 首次进入等待状态时记录开始时间
                                if (current_state != WAITING_LEFT)
                                {
                                    current_state = WAITING_LEFT;
                                    waiting_left_start_time = std::chrono::steady_clock::now();
                                    waiting_left_timeout = false; // 重置超时标记
                                    std::cout << "开始计时，等待障碍清除..." << std::endl;
                                }
                                // 已在等待状态，检查是否超时
                                else
                                {
                                    // 计算已等待时间
                                    auto now = std::chrono::steady_clock::now();
                                    auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(
                                                       now - waiting_left_start_time)
                                                       .count();

                                    // 超时检测（仅触发一次）
                                    if (elapsed >= WAITING_LEFT_TIMEOUT_SECONDS && !waiting_left_timeout)
                                    {
                                        waiting_left_timeout = true;
                                        std::cout << "双车道-前方+左侧障碍等待超时（"
                                                  << WAITING_LEFT_TIMEOUT_SECONDS << "秒），发送超时话题" << std::endl;

                                        // 发送超时话题（示例：使用ros消息）
                                        // rads::String timeout_msg;
                                        // timeout_msg.header.stamp = ros::Time::now();
                                        // timeout_msg.state = "WAITING_LEFT_TIMEOUT";
                                        // timeout_msg.elapsed_seconds = elapsed;
                                        // timeout_msg.location = "双车道-前方+左侧障碍";
                                        // obstacle_timeout_pub_.publish(timeout_msg);  // 假设已定义对应的publisher
                                    }
                                    // 未超时，输出等待提示
                                    else if (elapsed % 10 == 0)
                                    { // 每10秒提示一次
                                        std::cout << "已等待 " << elapsed << " 秒，继续等待障碍清除..." << std::endl;
                                    }
                                }
                                continue;
                            }
                            else
                            {
                                // 障碍清除时重置超时状态
                                waiting_left_timeout = false;
                                std::cout << "双车道-前方有障碍，变道至路线B" << std::endl;
                                current_state = CHANGING_TO_B;
                                need_re_snap = true;
                            }
                        }
                        else
                        {
                            // 无障碍时重置超时状态
                            waiting_left_timeout = false;
                        }
                    }
                    // 路线B逻辑（分阶段处理）
                    if (current_state == LANE_B)
                    {
                        // 刚进入B线，切换到计数状态并重置计数器
                        std::cout << "双车道-进入路线B，开始行驶" << LANE_B_REQUIRED_POINTS << "个点后检测雷达" << std::endl;
                        current_state = LANE_B_COUNTER;
                        lane_b_point_counter = 0; // 重置计数（从0开始）
                    }
                    // B线计数完成后，检测雷达1-3决定是否返回正常路线
                    else if (current_state == LANE_B_COUNTER && lane_b_point_counter >= LANE_B_REQUIRED_POINTS)
                    {
                        bool right_blocked = (Stop_lidar1 == 1 || Stop_lidar2 == 1 || Stop_lidar3 == 1);
                        if (!right_blocked)
                        {
                            std::cout << "双车道-路线B已完成" << LANE_B_REQUIRED_POINTS << "个点，右侧无障碍，返回正常路线" << std::endl;
                            current_state = CHANGING_TO_NORMAL;
                            need_re_snap = true;
                            lane_b_point_counter = 0; // 重置计数，准备下次使用
                        }
                        else
                        {
                            std::cout << "双车道-路线B已完成" << LANE_B_REQUIRED_POINTS << "个点，右侧有障碍，继续使用B线,Stop_lidar1:" << Stop_lidar1 << ",Stop_lidar2:" << Stop_lidar2 << ",Stop_lidar3:" << Stop_lidar3 << std::endl;
                            current_state = LANE_B; // 退出计数状态，保持B线行驶
                        }
                    }

                    // 路线B持续检测（仅在非计数状态下，避免干扰计数过程）
                    if (current_state == LANE_B)
                    {
                        bool right_blocked = (Stop_lidar1 == 1 || Stop_lidar2 == 1 || Stop_lidar3 == 1);
                        if (!right_blocked)
                        {
                            std::cout << "双车道-路线B行驶中，右侧障碍已清除，返回正常路线" << std::endl;
                            current_state = CHANGING_TO_NORMAL;
                            need_re_snap = true;
                        }
                    }
                }
                // 单车道逻辑（禁用变道，仅检测前方障碍）
                else
                {
                    // 单车道不检测Stop_lidar0，仅响应Stop_lidar（已在通用逻辑处理）
                    // 强制保持正常路线，不进入变道状态
                    if (current_state != NORMAL)
                    {
                        std::cout << "单车道-强制切换至正常路线，禁用变道功能" << std::endl;
                        current_state = NORMAL;
                        need_re_snap = true;
                    }
                }

                // 变道过程处理（仅双车道生效）
                switch (current_state)
                {
                case CHANGING_TO_B:
                {
                    if (Stop_lidar == 1)
                    {
                        std::cout << "变道至路线B过程中，前方有障碍，停车等待" << std::endl;
                        pubdrive(100, 2, 18000, 0);
                        continue;
                    }

                    const car &target_b = pathPointsB[snapped_index_b];
                    double dist_to_b = distance_to_point(target_b);
                    std::cout << "变道至路线B,距离吸附点：" << dist_to_b << std::endl;

                    if (dist_to_b < 1.5)
                    {
                        std::cout << "变道至路线B成功" << std::endl;
                        current_state = LANE_B; // 进入B线后触发计数准备
                    }
                    else
                    {
                        NDT_JDC = (lidar_rtk.matching_rate > 45.0)
                                      ? calculate_NDT_JDC(target_b)
                                      : rtkcalculate_NDT_JDC(target_b);
                    }
                    break;
                }
                case CHANGING_TO_NORMAL:
                {
                    if (Stop_lidar == 1)
                    {
                        std::cout << "变道至正常路线过程中，前方有障碍，停车等待" << std::endl;
                        pubdrive(100, 2, 18000, 0);
                        continue;
                    }

                    const car &target_normal = pathPoints[snapped_index];
                    double dist_to_normal = distance_to_point(target_normal);
                    std::cout << "返回正常路线,距离吸附点：" << dist_to_normal << std::endl;
                    if (dist_to_normal < 1.5)
                    {
                        std::cout << "返回正常路线成功" << std::endl;
                        current_state = NORMAL; // 回到正常路线，恢复原逻辑
                    }
                    else
                    {
                        NDT_JDC = (lidar_rtk.matching_rate > 45.0)
                                      ? calculate_NDT_JDC(target_normal)
                                      : rtkcalculate_NDT_JDC(target_normal);
                    }
                    break;
                }
                // 其他状态处理
                case WAITING_LEFT:
                {
                    bool left_blocked = (Stop_lidar5 == 1 || Stop_lidar6 == 1 || Stop_lidar7 == 1);
                    if (!left_blocked)
                    {
                        std::cout << "左侧障碍清除，变道至路线B" << std::endl;
                        current_state = CHANGING_TO_B;
                        need_re_snap = true;
                    }
                    else
                    {
                        std::cout << "左侧有障碍，无法变道至路线B,Stop_lidar5:" << Stop_lidar5 << ",Stop_lidar6:" << Stop_lidar6 << ",Stop_lidar7:" << Stop_lidar7 << std::endl;
                        pubdrive(100, 2, 18000, 0);
                        continue;
                    }
                    break;
                }
                case WAITING_RIGHT:
                {
                    bool right_blocked = (Stop_lidar1 == 1 || Stop_lidar2 == 1 || Stop_lidar3 == 1);
                    if (!right_blocked)
                    {
                        std::cout << "右侧障碍清除，返回正常路线" << std::endl;
                        current_state = CHANGING_TO_NORMAL;
                        need_re_snap = true;
                    }
                    else
                    {
                        std::cout << "右侧有障碍，无法返回正常路线" << std::endl;
                        pubdrive(100, 2, 18000, 0);
                        continue;
                    }
                    break;
                }
                default:
                    break;
                }

                // 速度调节
                if (Stop_lidar1 < LFRFLRRR || Stop_lidar2 < LFRFLRRR || Stop_lidar3 < LFRFLRRR || Stop_lidar4 < LFRFLRRR)
                {
                    if (speed_change_counter_update >= SPEED_CHANGE_INTERVAL)
                    {
                        speed_change_counter_update = 0;
                        speed = std::max(speed - 1, minspeed);
                    }
                }
                else
                {
                    if (speed_change_counter_update >= SPEED_CHANGE_INTERVAL)
                    {
                        speed_change_counter_update = 0;
                        speed = std::min(speed + 1, maxspeed);
                    }
                }

                // 角度修正
                if (PID_STATUS == 0)
                {
                    if (std::abs(NDT_JDC) < 30 && std::abs(NDT_JDC) > 0)
                    {
                        NDT_JDC_new = NDT_JDC * 1.5;
                    }
                    else if (std::abs(NDT_JDC) < 40 && std::abs(NDT_JDC) >= 30)
                    {
                        NDT_JDC_new = NDT_JDC * 1.2;
                    }
                    else if (std::abs(NDT_JDC) > 60)
                    {
                        NDT_JDC_new = NDT_JDC * 0.3;
                    }
                    else
                    {
                        NDT_JDC_new = NDT_JDC;
                    }
                }
                else
                {
                    NDT_JDC_new = NDT_JDC;
                }

                // 速度限制
                speed_new = static_cast<int>(cos(NDT_JDC_new / 114.6) * speed);
                if (std::abs(NDT_JDC) > 60 || std::abs(NDT_JDC) > 90)
                {
                    speed_new = 0;
                }

                // 输出驾驶信息
                std::cout << std::fixed << std::setprecision(2)
                          << "圈数: " << current_loop + 1 << "/" << loop_count << " | "
                          << "目标点: [" << target_point.num_point << "] | "
                          << "距离: " << distance << "米 | "
                          << "计算角度: " << NDT_JDC << "° | "
                          << "执行角度: " << NDT_JDC_new << "° | "
                          << "速度: " << speed_new << "KM/h | ";

                switch (current_state)
                {
                case NORMAL:
                    std::cout << "状态: 正常路线行驶";
                    break;
                case LANE_B:
                    std::cout << "状态: 路线B行驶(临时绕障)";
                    break;
                case LANE_B_COUNTER:
                    std::cout << "状态: 路线B行驶(计数中 "
                              << lane_b_point_counter << "/" << LANE_B_REQUIRED_POINTS << ")";
                    break;
                case CHANGING_TO_B:
                    std::cout << "状态: 变道中→路线B";
                    break;
                case CHANGING_TO_NORMAL:
                    std::cout << "状态: 变道中→正常路线";
                    break;
                case WAITING_LEFT:
                    std::cout << "状态: 正常路线等待(左侧障碍)";
                    break;
                case WAITING_RIGHT:
                    std::cout << "状态: 路线B等待(右侧障碍)";
                    break;
                default:
                    std::cout << "状态: 未知";
                    break;
                }
                std::cout << std::endl;

                // 发布驾驶指令
                pubdrive(0, 2, NDT_JDC_new * 100 + 18000.0, speed_new);
            }
            else
            {
                pubdrive(100, 2, 18000, 0);
            }

            usleep(20000);
        }

        // 圈数处理
        current_loop++;
        if (current_loop < loop_count)
        {
            std::cout << "准备开始第 " << current_loop + 1 << " 圈..." << std::endl;
            current_state = NORMAL;
            snapped_index = 0;
            snapped_index_b = 0;
            need_re_snap = true;
            speed = MINSPEED;
            usleep(1000000);
        }
    }

    // 任务结束逻辑
    rads::rads_acks msg;
    msg.ack = 3;
    msg.progress = "100,0,0,0,0";
    sradsack_pub_.publish(msg);
    std::cout << "无人驾驶任务结束" << std::endl;
    pubdrive(100, 2, 18000, 0);
    stopRadarThread_R.store(true);
    if (autocron_id != 0)
    {
        rads::rads_cron rads_cron_msg;
        rads_cron_msg.header.stamp = ros::Time::now();
        rads_cron_msg.ack = 1;
        pcron.publish(rads_cron_msg);
    }
}

// 自动驾驶
void radarThreadFunction(std::vector<car> pathPoints, int loop_count = 1, int autocron_id = 0)
{
    // PID
    EnhancedAnglePIDController enhanced_pid(CONFIG_KP, CONFIG_KI, CONFIG_KD, CONFIG_MIN_OUTPUT, CONFIG_MAX_OUTPUT, stop_car, Stop_lidar);
    double distance;
    double NDT_JDC;
    double NDT_JDC_new;
    int speed = MINSPEED;    // 初始速度
    int maxspeed = MAXSPEED; // 最大速度
    int minspeed = MINSPEED;
    int speed_new;
    int api_change_counter_update = 0;
    int speed_change_counter_update = 0;
    int current_loop = 0; // 当前循环次数

    while (current_loop < loop_count && !stopRadarThread.load())
    {
        int snapped_index = findSnappedWaypoint(pathPoints); // 吸附点
        if (snapped_index != -1)
        {
            const car &target_pointxf = pathPoints[snapped_index];
            if (lidar_rtk.matching_rate > 45.0)
            {
                std::cout << "当前吸附到点位: [" << target_pointxf.num_point << "] (" << target_pointxf.x << ", " << target_pointxf.y << ")" << std::endl;
            }
            else
            {
                std::cout << "当前吸附到点位: [" << target_pointxf.num_point << "] (" << target_pointxf.rtk_x << ", " << target_pointxf.rtk_y << ")" << std::endl;
            }
        }
        else
        {
            std::cout << "未找到有效吸附点" << std::endl;
        }

        while (snapped_index < pathPoints.size() && !stopRadarThread.load() && (lidar_rtk.matching_rate > 45 || lidar_rtk.posi_flag >= 4))
        {
            api_change_counter_update++;
            speed_change_counter_update++;
            // 1. 获取当前目标点
            const car &target_point = pathPoints[snapped_index];
            // 计算车到目标点的距离
            if (lidar_rtk.matching_rate > 45.0)
            {
                distance = distance_count(lidar_rtk.x, lidar_rtk.y, target_point.x, target_point.y);
            }
            else
            {
                distance = distance_count(lidar_rtk.rtk_x, lidar_rtk.rtk_y, target_point.rtk_x, target_point.rtk_y);
            }

            if (distance < DRIVE_DISRANCE)
            {
                std::cout << "已到达点位 [" << target_point.num_point << "]" << std::endl;
                snapped_index++; // 移向下一个点
                continue;
            }
            if (target_point.stop_flag == 1)
            {
                // 需要交互交互
                continue;
            }
            if (target_point.stop_time > 0)
            {
                // 需要等待
                sleep(target_point.stop_time);
                std::cout << "已到达点位 [" << target_point.num_point << "],需要等待：" << target_point.stop_time << "秒" << std::endl;
                snapped_index++; // 移向下一个点
                continue;
            }
            if (api_change_counter_update >= API_CHANGE_INTERVAL)
            {
                api_change_counter_update = 0; // 重置计数器
                publishSradsackProgress(snapped_index, pathPoints, speed_new, target_point.line_id, target_point.map_id);
            }

            if (PID_STATUS == 0)
            {
                // 正常计算角度
                if (lidar_rtk.matching_rate > 45.0)
                {
                    NDT_JDC = calculate_NDT_JDC(target_point);
                    ROS_INFO_STREAM(((stop_car != 1 || Stop_lidar == 1) ? "[STOPPED] " : "[MOVING] ")
                                    << "LIDAR Mode | Matching Rate: " << lidar_rtk.matching_rate
                                    << " | Output: " << NDT_JDC);
                }
                else
                {
                    NDT_JDC = rtkcalculate_NDT_JDC(target_point);
                    ROS_INFO_STREAM(((stop_car != 1 || Stop_lidar == 1) ? "[STOPPED] " : "[MOVING] ")
                                    << "RTK Mode | Posi Flag: " << lidar_rtk.posi_flag
                                    << " | Output: " << NDT_JDC);
                }
            }
            else
            {
                // 计算角度使用 PID
                if (lidar_rtk.matching_rate > 45.0)
                {
                    double raw_angle = calculate_NDT_JDC(target_point);
                    NDT_JDC = enhanced_pid.calculate(0.0, raw_angle);
                    ROS_INFO_STREAM(((stop_car != 1 || Stop_lidar == 1) ? "[STOPPED] " : "[MOVING] ")
                                    << "LIDAR Mode | Matching Rate: " << lidar_rtk.matching_rate
                                    << " | Raw Angle: " << raw_angle
                                    << " | PID Output: " << NDT_JDC);
                }
                else
                {
                    double raw_angle = rtkcalculate_NDT_JDC(target_point);
                    NDT_JDC = enhanced_pid.calculate(0.0, raw_angle);
                    ROS_INFO_STREAM(((stop_car != 1 || Stop_lidar == 1) ? "[STOPPED] " : "[MOVING] ")
                                    << "RTK Mode | Posi Flag: " << lidar_rtk.posi_flag
                                    << " | Raw Angle: " << raw_angle
                                    << " | PID Output: " << NDT_JDC);
                }
            }

            if (stop_car == 1)
            { // 正常驾驶
                while ((Stop_lidar == 1 || ladar_5 < FLLR || ladar_6 < FLLR) && !stopRadarThread.load())
                {
                    pubdrive(0, 2, 18000, 0); // int brake, int gear, int steering, int target_speed
                    std::cout << "前方有障碍，正在停车，当前速度: " << 0 << " KM/h" << std::endl;
                    speed = MINSPEED; // 初始速度
                    usleep(20000);
                }

                // 减速
                if (ladar_1 < LFRFLRRR || ladar_2 < LFRFLRRR || ladar_3 < LFRFLRRR || ladar_4 < LFRFLRRR)
                {
                    std::cout << "车辆正在无人驾驶,收到减速信号" << std::endl;
                    if (speed_change_counter_update >= SPEED_CHANGE_INTERVAL)
                    {
                        speed_change_counter_update = 0; // 重置计数器
                        if (speed > minspeed)
                        {
                            speed = speed - 1;
                        }
                    }
                }
                else
                {
                    // 加速--》直到最大速度
                    if (speed_change_counter_update >= SPEED_CHANGE_INTERVAL)
                    {
                        speed_change_counter_update = 0; // 重置计数器
                        if (speed < maxspeed)
                        {
                            speed = speed + 1;
                        }
                    }
                }

                if (PID_STATUS == 0)
                {
                    if (std::abs(NDT_JDC) < 30 && std::abs(NDT_JDC) > 0)
                    {
                        NDT_JDC_new = NDT_JDC * 1.5;
                    }
                    else if (std::abs(NDT_JDC) < 40 && std::abs(NDT_JDC) >= 30)
                    {
                        NDT_JDC_new = NDT_JDC * 1.2;
                    }
                    else if (std::abs(NDT_JDC) > 60)
                    {
                        NDT_JDC_new = NDT_JDC * 0.3;
                    }
                    else
                    {
                        NDT_JDC_new = NDT_JDC;
                    }
                }
                else
                {
                    NDT_JDC_new = NDT_JDC;
                }

                speed_new = cos(NDT_JDC_new / 114.6) * speed;
                if (std::abs(NDT_JDC) > 60)
                {
                    speed_new = 0;
                }
                if (std::abs(NDT_JDC) > 90)
                {
                    speed_new = 0;
                }

                // 输出导航信息current_loop < loop_count
                std::cout << std::fixed << std::setprecision(6) << "循环次数：" << loop_count << ",当前第" << current_loop + 1 << "次,总共：" << pathPoints.size() << ",导航至点位 [" << target_point.num_point << "]: (雷达:["
                          << target_point.x << ", " << target_point.y << "],rkt:[" << target_point.rtk_x << ", " << target_point.rtk_y << "])"
                          << " | 距离:" << distance << "米,计算角度：" << NDT_JDC << ",执行角度：" << NDT_JDC_new << "速度：" << speed_new << std::endl;
                pubdrive(0, 2, NDT_JDC_new * 100 + 18000.0, speed_new);
            }
            else
            {
                pubdrive(0, 2, 18000, 0);
            }
            usleep(20000);
        }

        current_loop++; // 增加循环计数
        if (current_loop < loop_count)
        {
            std::cout << "准备开始第 " << current_loop + 1 << " 次循环..." << std::endl;
            usleep(1000000); // 等待1秒再开始下一次循环
        }
    }

    rads::rads_acks msg;
    msg.ack = 3;
    int baifenbiend = 100;
    int lichengend = 0;
    int duoshaomiaoend = 0;
    int line_id = 0;
    int map_id = 0;
    std::string progressStr = std::to_string(baifenbiend) + "," + std::to_string(lichengend) + "," + std::to_string(duoshaomiaoend) + "," + std::to_string(line_id) + "," + std::to_string(map_id);
    msg.progress = progressStr;
    sradsack_pub_.publish(msg);
    std::cout << "正常--无人驾驶结束" << std::endl;
    // 发布任务结束
    pubdrive(100, 2, 18000, 0); // int brake, int gear, int steering, int target_speed
    stopRadarThread.store(true);
    // 发送确认消息
    if (autocron_id != 0)
    {
        rads::rads_cron rads_cron_msg;
        rads_cron_msg.header.stamp = ros::Time::now();
        rads_cron_msg.ack = 1;
        pcron.publish(rads_cron_msg);
    }
}
// 返航驾驶
void thread_Create_line_goback_play()
{
    double distance;
    double NDT_JDC;
    double NDT_JDC_new;
    int speed = MINSPEED;    // 初始速度
    int maxspeed = MAXSPEED; // 最大速度
    int minspeed = MINSPEED;
    int speed_new;
    int speed_change_counter_update = 0;
    double current_yaw, initial_yaw, target_yaw;
    double tolerance = 10.0;
    turnaround_state = TS_READY;
    try
    {
        auto pathPoints = fetchgobackDataFromDB(); // 可能抛出异常
        if (pathPoints.empty())
        {
            std::cout << "警告：未获取到返航路线数据" << std::endl;
            return;
        }
        int snapped_index = current_target_index.load();

        while (should_run_goback_play.load())
        {
            // 边界保护
            if (snapped_index < 0)
            {
                snapped_index = 0;
            }
            else if (snapped_index >= pathPoints.size())
            {
                snapped_index = pathPoints.size() - 1;
            }

            // 处理掉头状态
            if (need_turnaround)
            {
                switch (turnaround_state)
                {
                case TS_READY:
                    // 准备掉头，调整索引到正确的下一个点
                    if (lidar_rtk.posi_flag == 4 && lidar_rtk.head_flag == 4)
                    {
                        initial_yaw = lidar_rtk.heading;
                    }
                    else
                    {
                        initial_yaw = lidar_gobackmsg.ld_heading;
                    }

                    target_yaw = initial_yaw + 180.0;
                    if (snapped_index == 0)
                    {
                        // 起点掉头，只能向前进
                        if (pathPoints.size() > 1)
                        {
                            snapped_index = 1;
                            turnaround_state = TS_TURNING;
                            std::cout << "开始掉头，新目标索引: " << snapped_index << std::endl;
                        }
                        else
                        {
                            std::cout << "路径长度不足，无法掉头" << std::endl;
                            need_turnaround = false;
                        }
                    }
                    else if (snapped_index == pathPoints.size() - 1)
                    {
                        // 终点掉头，只能向后退
                        if (pathPoints.size() > 1)
                        {
                            snapped_index = pathPoints.size() - 2;
                            turnaround_state = TS_TURNING;
                            std::cout << "开始掉头，新目标索引: " << snapped_index << std::endl;
                        }
                        else
                        {
                            std::cout << "路径长度不足，无法掉头" << std::endl;
                            need_turnaround = false;
                        }
                    }
                    else
                    {
                        // 中间位置掉头，根据新方向调整
                        if (goback_forward)
                        {
                            // 掉头后正向行驶，确保下一个点在前方
                            snapped_index = std::min(static_cast<int>(pathPoints.size() - 1), snapped_index + 1);
                        }
                        else
                        {
                            // 掉头后反向行驶，确保下一个点在后方
                            snapped_index = std::max(0, snapped_index - 1);
                        }
                        turnaround_state = TS_TURNING;
                        std::cout << "开始掉头，新目标索引: " << snapped_index << std::endl;
                    }
                    current_target_index = snapped_index;
                    break;

                case TS_TURNING:
                {
                    // 执行掉头操作
                    // double current_yaw, initial_yaw, target_yaw;
                    // double tolerance = 10.0;

                    // 根据定位状态选择数据源
                    if (lidar_rtk.posi_flag == 4 && lidar_rtk.head_flag == 4)
                    {
                        current_yaw = lidar_rtk.heading;
                    }
                    else
                    {
                        current_yaw = lidar_gobackmsg.ld_heading;
                    }

                    speed = MINSPEED;

                    std::cout << "Current yaw: " << current_yaw << ", Target yaw: " << target_yaw
                              << ", 角度差: " << fabs(fmod(current_yaw - target_yaw, 360.0)) << std::endl;

                    if (fabs(fmod(current_yaw - target_yaw, 360.0)) <= tolerance)
                    {
                        // 掉头完成
                        turnaround_state = TS_COMPLETED;
                        need_turnaround = false;
                        pubdrive(0, 2, 18000, 0);
                        std::cout << "掉头完成，驶向新目标点" << std::endl;
                    }
                    else
                    {
                        if (stop_car == 1)
                        {
                            // 继续掉头
                            if (fabs(fmod(current_yaw - target_yaw, 360.0)) <= 90)
                            {
                                pubdrive(0, 2, 20 * 100 + 18000, 0);
                            }
                            else
                            {
                                pubdrive(0, 2, 40 * 100 + 18000, 0);
                            }
                        }
                        else
                        {
                            pubdrive(0, 2, 18000, 0);
                            std::cout << "掉头未完成，车辆暂停中" << std::endl;
                        }
                    }
                    break;
                }

                case TS_COMPLETED:
                    // 掉头已完成，重置状态
                    turnaround_state = TS_READY;
                    break;
                }

                // 如果正在掉头，跳过后续逻辑
                if (turnaround_state == TS_TURNING)
                {
                    usleep(20000);
                    continue;
                }
            }

            const go_back &target_point = pathPoints[snapped_index];
            speed_change_counter_update++;

            // 计算车到目标点的距离
            if (lidar_rtk.posi_flag == 4 && lidar_rtk.head_flag == 4)
            {
                distance = distance_count(lidar_rtk.rtk_x, lidar_rtk.rtk_y, target_point.rtk_x, target_point.rtk_y);
            }
            else
            {
                distance = distance_count(lidar_gobackmsg.x, lidar_gobackmsg.y, target_point.x, target_point.y);
            }

            if (distance < DRIVE_DISRANCE)
            {
                std::cout << "已到达点位 [" << target_point.num_point << "]" << std::endl;

                if (goback_forward)
                {
                    if (snapped_index < pathPoints.size() - 1)
                        snapped_index++;
                }
                else
                {
                    if (snapped_index > 0)
                        snapped_index--;
                }
                current_target_index = snapped_index;
                continue;
            }

            // 计算导航角度
            if (lidar_rtk.posi_flag == 4 && lidar_rtk.head_flag == 4)
            {
                NDT_JDC = goback_rtkcalculate_NDT_JDC(target_point);
                std::cout << "返航-当前使用RTK-posi_flag:" << lidar_rtk.posi_flag << std::endl;
            }
            else
            {
                NDT_JDC = goback_calculate_NDT_JDC(target_point);
                std::cout << "返航-当前使用雷达" << std::endl;
            }

            // 障碍物检测与速度控制
            if (stop_car == 1)
            { // 正常驾驶
                while ((Stop_lidar == 1 || ladar_5 < FLLR || ladar_6 < FLLR) && should_run_goback_play.load())
                {
                    pubdrive(0, 2, 18000, 0);
                    std::cout << ",前方有障碍，正在停车，当前速度: " << 0 << " KM/h" << std::endl;
                    speed = MINSPEED;
                    usleep(20000);
                }

                // 减速逻辑
                if (ladar_1 < LFRFLRRR || ladar_2 < LFRFLRRR || ladar_3 < LFRFLRRR || ladar_4 < LFRFLRRR)
                {
                    std::cout << "车辆正在无人驾驶,收到减速信号" << std::endl;
                    if (speed_change_counter_update >= SPEED_CHANGE_INTERVAL)
                    {
                        speed_change_counter_update = 0; // 重置计数器
                        if (speed > minspeed)
                        {
                            speed = speed - 1;
                        }
                    }
                }
                else
                {
                    // 加速--》直到最大速度
                    if (speed_change_counter_update >= SPEED_CHANGE_INTERVAL)
                    {
                        speed_change_counter_update = 0; // 重置计数器
                        if (speed < maxspeed)
                        {
                            speed = speed + 1;
                        }
                    }
                }

                // 角度调整与速度计算
                if (std::abs(NDT_JDC) < 30 && std::abs(NDT_JDC) > 0)
                {
                    NDT_JDC_new = NDT_JDC * 1.5;
                }
                else if (std::abs(NDT_JDC) < 40 && std::abs(NDT_JDC) >= 30)
                {
                    NDT_JDC_new = NDT_JDC * 1.2;
                }
                else if (std::abs(NDT_JDC) > 60)
                {
                    NDT_JDC_new = NDT_JDC * 0.3;
                }
                else
                {
                    NDT_JDC_new = NDT_JDC;
                }

                speed_new = cos(NDT_JDC_new / 114.6) * speed;
                if (std::abs(NDT_JDC) > 60)
                {
                    speed_new = 0;
                }
                if (std::abs(NDT_JDC) > 90)
                {
                    speed_new = 0;
                }

                // 输出导航信息
                std::cout << std::fixed << std::setprecision(6)
                          << "总共：" << pathPoints.size()
                          << ",导航至点位 [" << target_point.num_point
                          << "],当前的索引值[" << snapped_index << "]: ("
                          << target_point.x << ", " << target_point.y
                          << ",rkt:" << target_point.rtk_x << ", " << target_point.rtk_y << ")"
                          << " | 距离:" << distance
                          << "米,计算角度：" << NDT_JDC
                          << ",执行角度：" << NDT_JDC_new
                          << "速度：" << speed_new << std::endl;

                pubdrive(0, 2, NDT_JDC_new * 100 + 18000.0, speed_new);
            }
            else
            {
                pubdrive(0, 2, 18000, 0);
            }

            usleep(20000);
        }
    }
    catch (const std::exception &e)
    {
        std::cerr << "返航线程异常: " << e.what() << std::endl;
        should_run_goback_play.store(false);
    }
    std::cout << "返航结束" << std::endl;
    should_run_goback_play.store(false);
}
// 马踏飞燕
void thread_Create_line_all(int map_id)
{
    double last_x, last_y, last_heading, current_x, current_y, current_heading, distance, heading_diff;
    bool initial_use_rtk, use_rtk;
    int caidiannum_point = 1;
    initial_use_rtk = (lidar_rtk.matching_rate < 55) && (lidar_rtk.posi_flag == 4) && (lidar_rtk.head_flag == 4);
    last_x = initial_use_rtk ? lidar_rtk.rtk_x : lidar_rtk.x;
    last_y = initial_use_rtk ? lidar_rtk.rtk_y : lidar_rtk.y;
    last_heading = initial_use_rtk ? lidar_rtk.heading : lidar_rtk.ld_heading;
    while (should_run_all.load())
    {
        use_rtk = (lidar_rtk.matching_rate < 55) && (lidar_rtk.posi_flag == 4) && (lidar_rtk.head_flag == 4);
        current_x = use_rtk ? lidar_rtk.rtk_x : lidar_rtk.x;
        current_y = use_rtk ? lidar_rtk.rtk_y : lidar_rtk.y;
        current_heading = use_rtk ? lidar_rtk.heading : lidar_rtk.ld_heading;
        distance = distance_count(current_x, current_y, last_x, last_y);
        heading_diff = normalized_heading_diff(current_heading, last_heading);

        if (distance > AUTO_DISRANCE || heading_diff > HEADING_THRESHOLD)
        {
            last_x = current_x;
            last_y = current_y;
            last_heading = current_heading;
            rads::rads_acks rads_acks_msg;
            rads_acks_msg.header.stamp = ros::Time::now();
            rads_acks_msg.cmd = 888;
            rads_acks_msg.map_id = map_id;
            rads::rads_trackext trajectory_ext;
            trajectory_ext.x = lidar_rtk.x;
            trajectory_ext.y = lidar_rtk.y;
            trajectory_ext.z = lidar_rtk.z;
            trajectory_ext.yaw = lidar_rtk.ld_heading;
            trajectory_ext.ld_heading = lidar_rtk.ld_heading;
            trajectory_ext.matching_rate = lidar_rtk.matching_rate;
            trajectory_ext.rtk_x = lidar_rtk.rtk_x;
            trajectory_ext.rtk_y = lidar_rtk.rtk_y;
            trajectory_ext.rtk_z = lidar_rtk.rtk_z;
            trajectory_ext.longitude = lidar_rtk.longitude;
            trajectory_ext.latitude = lidar_rtk.latitude;
            trajectory_ext.heading = lidar_rtk.heading;
            trajectory_ext.posi_flag = lidar_rtk.posi_flag;
            trajectory_ext.head_flag = lidar_rtk.head_flag;
            rads_acks_msg.trajectory_ext.push_back(trajectory_ext);
            sradsack_pub_.publish(rads_acks_msg);
            cout << "创建基本路线,在地图号：" << map_id << ",踩点成功！当前点个数: " << caidiannum_point << endl;
            caidiannum_point++;
        }
        usleep(100);
    }
    cout << "结束创建基本路线,在地图号：" << map_id << ",一共：" << caidiannum_point - 1 << "个点" << endl;
}
// 记忆采集点
void thread_Create_line_goback()
{
    // 清空表
    delete_go_back();
    // 发布话题
    double last_x, last_y, last_heading, current_x, current_y, current_heading, distance, heading_diff;
    bool initial_use_rtk, use_rtk;
    int caidiannum_point = 1;
    initial_use_rtk = (lidar_rtk.posi_flag == 4) && (lidar_rtk.head_flag == 4);
    last_x = initial_use_rtk ? lidar_rtk.rtk_x : lidar_gobackmsg.x;
    last_y = initial_use_rtk ? lidar_rtk.rtk_y : lidar_gobackmsg.y;
    last_heading = initial_use_rtk ? lidar_rtk.heading : lidar_gobackmsg.ld_heading;
    rads::rads_acks rads_acks_msg;
    rads_acks_msg.header.stamp = ros::Time::now();
    rads_acks_msg.cmd = 999;
    rads::rads_trackext trajectory_ext;
    trajectory_ext.point_count = caidiannum_point;
    trajectory_ext.x = lidar_gobackmsg.x;
    trajectory_ext.y = lidar_gobackmsg.y;
    trajectory_ext.z = lidar_gobackmsg.z;
    trajectory_ext.rtk_x = lidar_rtk.rtk_x;
    trajectory_ext.rtk_y = lidar_rtk.rtk_y;
    trajectory_ext.rtk_z = lidar_rtk.rtk_z;
    trajectory_ext.yaw = lidar_rtk.ld_heading;
    trajectory_ext.ld_heading = lidar_rtk.ld_heading;
    trajectory_ext.longitude = lidar_rtk.longitude;
    trajectory_ext.latitude = lidar_rtk.latitude;
    trajectory_ext.heading = lidar_rtk.heading;
    trajectory_ext.posi_flag = lidar_rtk.posi_flag;
    trajectory_ext.head_flag = lidar_rtk.head_flag;
    rads_acks_msg.trajectory_ext.push_back(trajectory_ext);
    sradsack_pub_.publish(rads_acks_msg);
    cout << "创建记忆路线,踩点成功！当前点个数: " << caidiannum_point << " | 模式:" << (use_rtk ? "RTK" : "激光")
         << ",distance:" << distance << ",heading_diff:" << heading_diff << endl;
    caidiannum_point++;
    while (should_run_goback.load())
    {
        use_rtk = (lidar_rtk.posi_flag == 4) && (lidar_rtk.head_flag == 4);
        current_x = use_rtk ? lidar_rtk.rtk_x : lidar_gobackmsg.x;
        current_y = use_rtk ? lidar_rtk.rtk_y : lidar_gobackmsg.y;
        current_heading = use_rtk ? lidar_rtk.heading : lidar_gobackmsg.ld_heading;
        distance = distance_count(current_x, current_y, last_x, last_y);
        heading_diff = normalized_heading_diff(current_heading, last_heading);

        if (distance > AUTO_DISRANCE || heading_diff > HEADING_THRESHOLD)
        {
            last_x = current_x;
            last_y = current_y;
            last_heading = current_heading;
            rads::rads_acks rads_acks_msg;
            rads_acks_msg.header.stamp = ros::Time::now();
            rads_acks_msg.cmd = 999;
            rads::rads_trackext trajectory_ext;
            trajectory_ext.point_count = caidiannum_point;
            trajectory_ext.x = lidar_gobackmsg.x;
            trajectory_ext.y = lidar_gobackmsg.y;
            trajectory_ext.z = lidar_gobackmsg.z;
            trajectory_ext.rtk_x = lidar_rtk.rtk_x;
            trajectory_ext.rtk_y = lidar_rtk.rtk_y;
            trajectory_ext.rtk_z = lidar_rtk.rtk_z;
            trajectory_ext.yaw = lidar_rtk.ld_heading;
            trajectory_ext.ld_heading = lidar_rtk.ld_heading;
            trajectory_ext.longitude = lidar_rtk.longitude;
            trajectory_ext.latitude = lidar_rtk.latitude;
            trajectory_ext.heading = lidar_rtk.heading;
            trajectory_ext.posi_flag = lidar_rtk.posi_flag;
            trajectory_ext.head_flag = lidar_rtk.head_flag;
            rads_acks_msg.trajectory_ext.push_back(trajectory_ext);
            sradsack_pub_.publish(rads_acks_msg);
            cout << "创建记忆路线,踩点成功！当前点个数: " << caidiannum_point << " | 模式:" << (use_rtk ? "RTK" : "激光")
                 << ",distance:" << distance << ",heading_diff:" << heading_diff << endl;
            caidiannum_point++;
        }
        usleep(100);
    }
    cout << "结束记忆路线,一共：" << caidiannum_point - 1 << "个点" << endl;
}
// 踩点带路线和地图B
void thread_Create_lineB(int map_id, int path_id, string pro_nema)
{
    double last_x, last_y, last_heading, current_x, current_y, current_heading, distance, heading_diff;
    bool initial_use_rtk, use_rtk;
    int caidiannum_point = 1;
    initial_use_rtk = (lidar_rtk.matching_rate < 55) && (lidar_rtk.posi_flag == 4) && (lidar_rtk.head_flag == 4);
    last_x = initial_use_rtk ? lidar_rtk.rtk_x : lidar_rtk.x;
    last_y = initial_use_rtk ? lidar_rtk.rtk_y : lidar_rtk.y;
    last_heading = initial_use_rtk ? lidar_rtk.heading : lidar_rtk.ld_heading;

    while (should_runB.load())
    {
        use_rtk = (lidar_rtk.matching_rate < 55) && (lidar_rtk.posi_flag == 4) && (lidar_rtk.head_flag == 4);

        current_x = use_rtk ? lidar_rtk.rtk_x : lidar_rtk.x;
        current_y = use_rtk ? lidar_rtk.rtk_y : lidar_rtk.y;
        current_heading = use_rtk ? lidar_rtk.heading : lidar_rtk.ld_heading;
        distance = distance_count(current_x, current_y, last_x, last_y);
        heading_diff = normalized_heading_diff(current_heading, last_heading);

        if (distance > AUTO_DISRANCE || heading_diff > HEADING_THRESHOLD)
        {
            last_x = current_x;
            last_y = current_y;
            last_heading = current_heading;
            rads::rads_acks rads_acks_msg;
            rads_acks_msg.header.stamp = ros::Time::now();
            rads_acks_msg.pro_nema = pro_nema;
            rads_acks_msg.cmd = 1024;
            rads_acks_msg.cmd_opt = map_id;
            rads_acks_msg.ext1 = path_id;
            rads_acks_msg.ext2 = 1;
            rads_acks_msg.ack = 2;
            rads_acks_msg.map_id = map_id;
            rads_acks_msg.path_id = path_id;
            rads::rads_trackext trajectory_ext;
            trajectory_ext.point_count = caidiannum_point;
            trajectory_ext.x = lidar_rtk.x;
            trajectory_ext.y = lidar_rtk.y;
            trajectory_ext.z = lidar_rtk.z;
            trajectory_ext.rtk_x = lidar_rtk.rtk_x;
            trajectory_ext.rtk_y = lidar_rtk.rtk_y;
            trajectory_ext.rtk_z = lidar_rtk.rtk_z;
            trajectory_ext.yaw = lidar_rtk.ld_heading;
            trajectory_ext.ld_heading = lidar_rtk.ld_heading;
            trajectory_ext.matching_rate = lidar_rtk.matching_rate;
            trajectory_ext.longitude = lidar_rtk.longitude;
            trajectory_ext.latitude = lidar_rtk.latitude;
            trajectory_ext.heading = lidar_rtk.heading;
            trajectory_ext.posi_flag = lidar_rtk.posi_flag;
            trajectory_ext.head_flag = lidar_rtk.head_flag;
            trajectory_ext.ext = modecaidian;
            rads_acks_msg.trajectory_ext.push_back(trajectory_ext);
            sradsack_pub_.publish(rads_acks_msg);
            cout << "创建路线B,在地图号：" << map_id << " ,创建了线路号B：" << path_id << ",踩点成功！当前点个数: " << caidiannum_point << endl;
            caidiannum_point++;
        }
        usleep(100);
    }
    cout << "结束创建路线,在地图号B：" << map_id << " ,创建了线路号B：" << path_id << ",一共：" << caidiannum_point - 1 << "个点" << endl;
}
// 踩点带路线和地图
void thread_Create_line(int map_id, int path_id, string pro_nema)
{
    double last_x, last_y, last_heading, current_x, current_y, current_heading, distance, heading_diff;
    bool initial_use_rtk, use_rtk;
    int caidiannum_point = 1;
    initial_use_rtk = (lidar_rtk.matching_rate < 55) && (lidar_rtk.posi_flag == 4) && (lidar_rtk.head_flag == 4);
    last_x = initial_use_rtk ? lidar_rtk.rtk_x : lidar_rtk.x;
    last_y = initial_use_rtk ? lidar_rtk.rtk_y : lidar_rtk.y;
    last_heading = initial_use_rtk ? lidar_rtk.heading : lidar_rtk.ld_heading;

    while (should_run.load())
    {
        use_rtk = (lidar_rtk.matching_rate < 55) && (lidar_rtk.posi_flag == 4) && (lidar_rtk.head_flag == 4);

        current_x = use_rtk ? lidar_rtk.rtk_x : lidar_rtk.x;
        current_y = use_rtk ? lidar_rtk.rtk_y : lidar_rtk.y;
        current_heading = use_rtk ? lidar_rtk.heading : lidar_rtk.ld_heading;
        distance = distance_count(current_x, current_y, last_x, last_y);
        heading_diff = normalized_heading_diff(current_heading, last_heading);

        if (distance > AUTO_DISRANCE || heading_diff > HEADING_THRESHOLD)
        {
            last_x = current_x;
            last_y = current_y;
            last_heading = current_heading;
            rads::rads_acks rads_acks_msg;
            rads_acks_msg.header.stamp = ros::Time::now();
            rads_acks_msg.pro_nema = pro_nema;
            rads_acks_msg.cmd = 12;
            rads_acks_msg.cmd_opt = map_id;
            rads_acks_msg.ext1 = path_id;
            rads_acks_msg.ext2 = 1;
            rads_acks_msg.ack = 2;
            rads_acks_msg.map_id = map_id;
            rads_acks_msg.path_id = path_id;
            rads::rads_trackext trajectory_ext;
            trajectory_ext.point_count = caidiannum_point;
            trajectory_ext.x = lidar_rtk.x;
            trajectory_ext.y = lidar_rtk.y;
            trajectory_ext.z = lidar_rtk.z;
            trajectory_ext.rtk_x = lidar_rtk.rtk_x;
            trajectory_ext.rtk_y = lidar_rtk.rtk_y;
            trajectory_ext.rtk_z = lidar_rtk.rtk_z;
            trajectory_ext.yaw = lidar_rtk.ld_heading;
            trajectory_ext.ld_heading = lidar_rtk.ld_heading;
            trajectory_ext.matching_rate = lidar_rtk.matching_rate;
            trajectory_ext.longitude = lidar_rtk.longitude;
            trajectory_ext.latitude = lidar_rtk.latitude;
            trajectory_ext.heading = lidar_rtk.heading;
            trajectory_ext.posi_flag = lidar_rtk.posi_flag;
            trajectory_ext.head_flag = lidar_rtk.head_flag;
            trajectory_ext.ext = modecaidian;
            rads_acks_msg.trajectory_ext.push_back(trajectory_ext);
            sradsack_pub_.publish(rads_acks_msg);
            cout << "创建路线,在地图号：" << map_id << " ,创建了线路号：" << path_id << ",踩点成功！当前点个数: " << caidiannum_point << endl;
            caidiannum_point++;
        }
        usleep(100);
    }
    cout << "结束创建路线,在地图号：" << map_id << " ,创建了线路号：" << path_id << ",一共：" << caidiannum_point - 1 << "个点" << endl;
}
// 接受网络指令
void pradscmdCallback(const rads::rads_cmd::ConstPtr &msg)
{
    sradsack_fast sradsack_fast_data;
    sradsack_fast_data.pro_nema = msg->pro_nema;
    sradsack_fast_data.cmd = msg->pro_cmd;
    sradsack_fast_data.cmd_opt = msg->pro_cmd_opt;
    sradsack_fast_data.ext1 = msg->pro_ext1;
    sradsack_fast_data.ext2 = msg->pro_ext2;
    rads::rads_cron rads_cron_msg;
    ROS_INFO("Received pradscmd message:");
    ROS_INFO_STREAM("pro_nema: " << msg->pro_nema);
    ROS_INFO_STREAM("pro_head: " << msg->pro_head);
    ROS_INFO_STREAM("pro_vehsn: " << msg->pro_vehsn);
    ROS_INFO_STREAM("pro_cmd: " << msg->pro_cmd);
    ROS_INFO_STREAM("pro_cmd_opt: " << msg->pro_cmd_opt);
    ROS_INFO_STREAM("pro_ext1: " << msg->pro_ext1);
    ROS_INFO_STREAM("pro_ext2: " << msg->pro_ext2);
    for (int i = 0; i < msg->data.size(); ++i)
    {
        ROS_INFO_STREAM("data[" << i << "]: " << msg->data[i]);
    }
    if (msg->pro_cmd == 299)
    {
        std::cout << "****处理合并指令299****" << std::endl;
        // 检查地图是否需要切换
        if (getmap_id != msg->pro_cmd_opt)
        {
            getmap_id = msg->pro_cmd_opt;
            map_ck(msg->pro_cmd_opt);
            sleep(3);
        }
        // 检查数据长度是否足够
        if (msg->data.size() != 2 * msg->pro_ext2)
        {
            std::cout << "错误：合并指令数据长度不足" << std::endl;
            rads::rads_cron rads_cron_msg;
            rads_cron_msg.header.stamp = ros::Time::now();
            rads_cron_msg.ack = 2;
            pcron.publish(rads_cron_msg);
            return;
        }

        // 1. 提取站点编号（前N个数据）
        int station_count = msg->pro_ext2;
        std::vector<int> point_ids;
        for (int i = 0; i < station_count; i++)
        {
            point_ids.push_back(msg->data[i]);
        }

        // 2. 提取站点描述（后N个数据）
        std::vector<int> station_descriptions;
        for (int i = station_count; i < 2 * station_count; i++)
        {
            station_descriptions.push_back(msg->data[i]);
        }

        // 4. 打印路线信息
        std::cout << "****获取数据 ***去往" << msg->pro_cmd_opt << "号地图,"
                  << msg->pro_ext1 << "号线的";
        for (int i = 0; i < point_ids.size(); ++i)
        {
            std::cout << "第" << point_ids[i] << "号点位,";
        }
        std::cout << "***************" << std::endl;

        // 5. 准备参数
        int map_id = msg->pro_cmd_opt;
        std::string line_id = std::to_string(msg->pro_ext1);

        // 6. 获取路径点并寻找吸附点
        auto pathPoints = fetchCarDataFromDB(msg->pro_cmd_opt, msg->pro_ext1);
        // int snapped_index = findSnappedWaypoint(pathPoints);
        std::vector<int> nearest_points = findcronpoint(pathPoints);
        int snapped_index = findNearestToFirst(nearest_points, msg->data[0]);
        std::cout << "吸附的点位:" << snapped_index << std::endl;

        // 7. 处理吸附点无效的情况
        if (snapped_index == -1)
        {

            rads_cron_msg.header.stamp = ros::Time::now();
            rads_cron_msg.ack = 2;
            pcron.publish(rads_cron_msg);
            return;
        }

        // 8. 获取新路径数据
        //auto carData = CronfetchCarDataFromDB(map_id, line_id, point_ids, snapped_index);

        CarDataResult carData = CronfetchCarDataFromDB(map_id, line_id, point_ids, snapped_index);

        std::cout << "主数据数量: " << carData.primaryData.size() << std::endl;
        for (const auto &data : carData.primaryData)
        {
            std::cout << "点位:" << data.num_point << ",stop_flag:" << data.stop_flag
                      << ",stop_time:" << data.stop_time  << ",x:" << data.x
                      << ",y:" << data.y << ",pid:" << data.pid << ",stopid:" << data.stopid << ",describe:" << data.describe << std::endl;
        }

        std::cout << "辅助数据数量: " << carData.secondaryData.size() << std::endl;
        for (const auto &data : carData.secondaryData)
        {
            std::cout << "点位:" << data.num_point << ",stop_flag:" << data.stop_flag
                      << ",stop_time:" << data.stop_time  << ",x:" << data.x
                      << ",y:" << data.y << ",pid:" << data.pid << ",stopid:" << data.stopid << ",describe:" << data.describe << std::endl;
        }



        // 获取B路线路径数据

        // 10. 修改站点停留信息
        // std::cout << "站点停留信息更新前:" << std::endl;
        // for (const auto& data : carData) {
        //     std::cout << "点位:" << data.num_point << ",stop_flag:" << data.stop_flag
        //               << ",stop_time:" << data.stop_time  << ",pid:" << data.pid << std::endl;
        // }

        // 打印station_descriptions内容
        // std::cout << "站点描述数据: ";
        // for (int desc : station_descriptions) {
        //     std::cout << desc << " ";
        // }
        // std::cout << std::endl;

        // 按pid精确匹配更新
        for (size_t i = 0; i < point_ids.size(); i++)
        {
            int targetPid = i + 1;
            int description = station_descriptions[i];

            bool found = false;
            for (auto &data : carData.primaryData)
            {
                if (data.pid == targetPid)
                {
                    // std::cout << "更新点位 " << data.num_point
                    //           << " (pid=" << data.pid << ") 停留时间为 " << description << std::endl;
                    data.stop_time = description;
                    found = true;
                    break;
                }
            }

            if (!found)
            {
                std::cout << "警告: 未找到pid=" << targetPid << "的站点" << std::endl;
            }
        }
        std::cout << "站点停留信息更新后:" << std::endl;
        for (const auto &data : carData.primaryData)
        {
            std::cout << "点位:" << data.num_point << ",stop_flag:" << data.stop_flag
                      << ",stop_time:" << data.stop_time << std::endl;
        }

        // 检查数据是否为空
        if (carData.primaryData.empty() || carData.secondaryData.empty())
        {
            std::cout << "错误: 路线数据为空，无法启动自动驾驶" << std::endl;
            rads_cron_msg.ack = 2;
            pcron.publish(rads_cron_msg);
            return;
        }
        // 检测当其是否有无人驾驶任务
         if (!stopRadarThread.load() || !stopRadarThread_R.load())
        {
            std::cout << "错误: 当前有无人驾驶任务，无法启动自动驾驶" << std::endl;
            rads_cron_msg.ack = 2;
            pcron.publish(rads_cron_msg);
            return;
        }
        // 启动自动驾驶
        if(RADS_B == 0){
            if (stopRadarThread.load())
            {
                stop_car = 1;
                stopRadarThread.store(false);
                int loop_count = 1;
                std::thread radarThread(radarThreadFunction, carData.primaryData, loop_count, cron_id);
                radarThread.detach();
            }
        }

        if(RADS_B == 1){
                if(stopRadarThread_R.load()){
                   
                    stop_car = 1;
                    stopRadarThread_R.store(false);
                    int loop_count = 1;
                    std::thread radarThread(radarThreadFunction_R, carData.primaryData,carData.secondaryData, loop_count, cron_id);
                    radarThread.detach();
                }
        }
        std::cout << "****合并指令执行完成****" << std::endl;
    }
    if (msg->pro_cmd == 29)
    {
        // 2. 打印路线信息
        std::cout << "****获取数据 ***去往" << msg->pro_cmd_opt << "号地图," << msg->pro_ext1 << "号线的";
        if (getmap_id != msg->pro_cmd_opt)
        {
            getmap_id = msg->pro_cmd_opt;
            map_ck(msg->pro_cmd_opt);
            sleep(3);
        }
        for (int i = 0; i < msg->data.size(); ++i)
        {
            std::cout << "第" << msg->data[i] << "号点位,";
        }
        std::cout << "***************" << std::endl;

        // 3. 准备参数
        int map_id = msg->pro_cmd_opt;
        std::string line_id = std::to_string(msg->pro_ext1);
        std::vector<int> point_ids(msg->data.begin(), msg->data.end());

        // 5. 获取路径点并寻找吸附点
        auto pathPoints = fetchCarDataFromDB(msg->pro_cmd_opt, msg->pro_ext1);
        // int snapped_index = findcronpoint(pathPoints);
        std::vector<int> nearest_points = findcronpoint(pathPoints);
        int snapped_index = findNearestToFirst(nearest_points, msg->data[0]);
        std::cout << "吸附的点位:" << snapped_index << std::endl;

        // 6. 处理吸附点无效的情况
        if (snapped_index == -1)
        {
            rads::rads_cron rads_cron_msg;
            rads_cron_msg.header.stamp = ros::Time::now();
            rads_cron_msg.ack = 2; // 表示失败
            pcron.publish(rads_cron_msg);
            return; // 直接返回，不执行后续代码
        }

        // 7. 获取新的路径数据
        //auto carData = CronfetchCarDataFromDB(map_id, line_id, point_ids, snapped_index);
        CarDataResult carData = CronfetchCarDataFromDB(map_id, line_id, point_ids, snapped_index);

        std::cout << "主数据数量: " << carData.primaryData.size() << std::endl;
        for (const auto &data : carData.primaryData)
        {
            std::cout << "点位:" << data.num_point << ",x:" << data.x
                      << ",y:" << data.y << ",pid:" << data.pid << ",stopid:" << data.stopid << ",describe:" << data.describe << std::endl;
        }

        std::cout << "辅助数据数量: " << carData.secondaryData.size() << std::endl;
        for (const auto &data : carData.secondaryData)
        {
            std::cout << "点位:" << data.num_point << ",x:" << data.x
                       << ",y:" << data.y << ",pid:" << data.pid << ",stopid:" << data.stopid << ",describe:" << data.describe << std::endl;
        }
        // 检查数据是否为空
        if (carData.primaryData.empty() || carData.secondaryData.empty())
        {
            std::cout << "错误: 路线数据为空，无法启动自动驾驶" << std::endl;
            rads_cron_msg.ack = 2;
            pcron.publish(rads_cron_msg);
            return;
        }
        // 检测当其是否有无人驾驶任务
        if (!stopRadarThread.load() || !stopRadarThread_R.load())
        {
            std::cout << "错误: 当前有无人驾驶任务，无法启动自动驾驶" << std::endl;
            rads_cron_msg.ack = 2;
            pcron.publish(rads_cron_msg);
            return;
        }

        // 启动自动驾驶
        if(RADS_B == 0){
            if (stopRadarThread.load())
            {
                stop_car = 1;
                stopRadarThread.store(false);
                int loop_count = 1;
                std::thread radarThread(radarThreadFunction, carData.primaryData, loop_count, cron_id);
                radarThread.detach();
            }
        }

        if(RADS_B == 1){
                if(stopRadarThread_R.load()){
                    stop_car = 1;
                    stopRadarThread_R.store(false);
                    int loop_count = 1;
                    std::thread radarThread(radarThreadFunction_R, carData.primaryData,carData.secondaryData, loop_count, cron_id);
                    radarThread.detach();
                }
        }
    }
    // 任意点导航
    if (msg->pro_cmd == 30)
    {
        std::cout << "***********************" << "任意点导航开始" << "*****************************" << std::endl;
        // 地图    msg->pro_cmd_opt;
        // 点数    msg->pro_ext1;
        // msg->pro_ext2  0 ：预览  1：执行   2：执行带返航
        rads::rads_acks acksmsg;
        rads::rads_trackext trajectory_ext;
        acksmsg.header.stamp = ros::Time::now();
        acksmsg.header.frame_id = "sradsack_map";
        acksmsg.pro_nema = msg->pro_nema;
        acksmsg.cmd = msg->pro_cmd;
        acksmsg.cmd_opt = msg->pro_cmd_opt;
        acksmsg.ext1 = msg->pro_ext1;
        acksmsg.ext2 = msg->pro_ext2;
        acksmsg.ack = 2;
        acksmsg.map_id = msg->pro_cmd_opt;
        double x1, y1, x2, y2;
        std::vector<std::vector<int16_t>> all_paths;
        all_paths.clear();
        if (msg->pro_ext2 != 0) // 预览不清除表
        {
            // 清空表
        }
        // 获取所有基本点
        auto pathPoints = fetchPointallDataFromDB(msg->pro_cmd_opt);

        if (msg->pro_ext1 > 0 && msg->data.size() >= msg->pro_ext1 * 2) // 执行
        {
            x1 = lidar_rtk.x;
            y1 = lidar_rtk.y;
            x2 = msg->data[0] * 0.01;
            y2 = msg->data[1] * 0.01;
            all_paths.push_back(getPath_anypoint(x1, y1, x2, y2, pathPoints));
            for (int i = 0; i < (msg->pro_ext1 - 1) * 2; i += 2)
            {
                x1 = msg->data[i] * 0.01;
                y1 = msg->data[i + 1] * 0.01;
                x2 = msg->data[i + 2] * 0.01;
                y2 = msg->data[i + 3] * 0.01;
                all_paths.push_back(getPath_anypoint(x1, y1, x2, y2, pathPoints));
            }
        }

        // 遍历 all_paths 并打印每个路径点
        int num = 1;
        for (size_t i = 0; i < all_paths.size(); ++i)
        {
            for (size_t j = 0; j < all_paths[i].size(); ++j)
            {
                auto it = std::find_if(pathPoints.begin(), pathPoints.end(), [&](const point_all &p)
                                       { return p.num == all_paths[i][j] + 1; });
                if (it != pathPoints.end())
                {
                    const point_all &pointall = *it;
                    trajectory_ext.point_count = num;
                    trajectory_ext.alltotal = all_paths[i].size();
                    trajectory_ext.x = pointall.x;
                    trajectory_ext.y = pointall.y;
                    trajectory_ext.z = pointall.z;
                    trajectory_ext.ld_heading = pointall.lidar_heading;
                    trajectory_ext.matching_rate = pointall.ndt_matching;
                    trajectory_ext.rtk_x = pointall.rtk_x;
                    trajectory_ext.rtk_y = pointall.rtk_y;
                    trajectory_ext.rtk_z = pointall.rtk_z;
                    trajectory_ext.longitude = pointall.lng;
                    trajectory_ext.latitude = pointall.lat;
                    trajectory_ext.heading = pointall.rtk_heading;
                    trajectory_ext.posi_flag = pointall.position_flag;
                    trajectory_ext.head_flag = pointall.orientation_flag;
                    acksmsg.trajectory_ext.push_back(trajectory_ext);
                    num++;
                }
            }
        }
        sradsack_pub_.publish(acksmsg);

        stopRadarThread.store(true);
        sleep(2);
        // 获取该路径上的所有点位
        auto point_data_points = fetchPointdataDataFromDB();
        // 启动自动驾驶线程
        stopRadarThread.store(false);
        int loop_count = 1; // 循环次数（可改为参数或配置）
        std::thread radarThread(radarThreadFunction, point_data_points, loop_count, cron_id);
        radarThread.detach();
    }

    // 创建基本线路（马踏飞燕）
    if (msg->pro_cmd == 7)
    {
        if (msg->pro_ext2 == 1 && !thread_instance_createline_all.joinable())
        {
            std::cout << "***********************" << "创建基本线路,地图编号:" << msg->pro_cmd_opt << "*****************************" << std::endl;
            // 检查地图是否需要切换
            if (getmap_id != msg->pro_cmd_opt)
            {
                getmap_id = msg->pro_cmd_opt;
                map_ck(msg->pro_cmd_opt);
                sleep(3);
            }
            should_run_all.store(true);
            thread_instance_createline_all = std::thread(std::bind(thread_Create_line_all, msg->pro_cmd_opt));
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }

        if (msg->pro_ext2 == 0 && thread_instance_createline_all.joinable())
        {
            should_run_all.store(false);
            if (thread_instance_createline_all.joinable())
            {
                thread_instance_createline_all.join(); // 等待线程结束
            }
            std::cout << "***********************" << "结束创建基本线路,地图编号:" << msg->pro_cmd_opt << "*****************************" << std::endl;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 终止任务
    if (msg->pro_cmd == 20)
    {
        std::cout << "***********************" << "终止任务" << "*****************************" << std::endl;
        stopRadarThread.store(true);   // 发送停止信号
        stopRadarThread_R.store(true); // 发送停止信号
        sradsack_fast_data.map_id = msg->pro_cmd_opt;
        sradsack_fast_data.ack = 1;
        publishacks(sradsack_pub_, sradsack_fast_data);
    }

    // 调度路线
    if (msg->pro_cmd == 5)
    {
        std::cout << "**********去往" << msg->pro_cmd_opt << "号地图," << msg->pro_ext1 << "号线的" << msg->pro_ext2 << "号点位***************" << std::endl;

        // 获取该路径上的所有点位
        auto pathPoints = fetchCarDataFromDB(msg->pro_cmd_opt, msg->pro_ext1);
        auto pathPointsB = BfetchCarDataFromDB(msg->pro_cmd_opt, msg->pro_ext1);

        sradsack_fast_data.ack = 1;
        sradsack_fast_data.map_id = msg->pro_cmd_opt;
        sradsack_fast_data.path_id = msg->pro_ext1;
        publishacks(sradsack_pub_, sradsack_fast_data);

             // 检测当其是否有无人驾驶任务
        if (!stopRadarThread.load() || !stopRadarThread_R.load())
        {
            std::cout << "错误: 当前有无人驾驶任务，无法启动自动驾驶" << std::endl;
            rads_cron_msg.ack = 2;
            pcron.publish(rads_cron_msg);
            return;
        }

        if (RADS_B == 0)
        {
            if (stopRadarThread.load())
            {
                // 检查地图是否需要切换
                if (getmap_id != msg->pro_cmd_opt)
                {
                    getmap_id = msg->pro_cmd_opt;
                    map_ck(msg->pro_cmd_opt);
                    sleep(3);
                }
                stop_car = 1;
                stopRadarThread.store(false);
                int loop_count = 1;
                std::thread radarThread(radarThreadFunction, pathPoints, loop_count, cron_id);
                radarThread.detach();
            }
        }

        if (RADS_B == 1)
        {
            if (stopRadarThread_R.load())
            {

                // 检查地图是否需要切换
                if (getmap_id != msg->pro_cmd_opt)
                {
                    getmap_id = msg->pro_cmd_opt;
                    map_ck(msg->pro_cmd_opt);
                    sleep(3);
                }
                stop_car = 1;
                stopRadarThread_R.store(false);
                int loop_count = 1;
                std::thread radarThread(radarThreadFunction_R, pathPoints, pathPointsB, loop_count, cron_id);
                radarThread.detach();
            }
        }
    }
    // 调度路线--循环
    if (msg->pro_cmd == 6)
    {
        std::cout << "**********去往" << msg->pro_cmd_opt << "号地图," << msg->pro_ext1 << "号线，循环次数：" << msg->pro_ext2 << "***************" << std::endl;
        // 获取该路径上的所有点位
        auto pathPoints = fetchCarDataFromDB(msg->pro_cmd_opt, msg->pro_ext1);
        sradsack_fast_data.ack = 1;
        sradsack_fast_data.map_id = msg->pro_cmd_opt;
        sradsack_fast_data.path_id = msg->pro_ext1;
        publishacks(sradsack_pub_, sradsack_fast_data);

        if (stopRadarThread.load())
        {

            // 检查地图是否需要切换
            if (getmap_id != msg->pro_cmd_opt)
            {
                getmap_id = msg->pro_cmd_opt;
                map_ck(msg->pro_cmd_opt);
                sleep(3);
            }
            stop_car = 1;
            stopRadarThread.store(false);
            int loop_count = 1;
            std::thread radarThread(radarThreadFunction, pathPoints, loop_count, cron_id);
            radarThread.detach();
        }
    }
    // 临时停车
    if (msg->pro_cmd == 8)
    {
        if (msg->pro_ext2 == 1)
        {
            std::cout << "***********************" << "临时停车" << "*****************************" << std::endl;
            stop_car = 2;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
        if (msg->pro_ext2 == 0)
        {
            std::cout << "***********************" << "解除临时停车" << "*****************************" << std::endl;
            stop_car = 1;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 紧急制动停车
    if (msg->pro_cmd == 9)
    {
        if (msg->pro_ext2 == 1)
        {
            std::cout << "***********************" << "紧急制动停车" << "*****************************" << std::endl;
            stop_car = 3;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
        if (msg->pro_ext2 == 0)
        {
            std::cout << "***********************" << "解除紧急制动停车" << "*****************************" << std::endl;
            stop_car = 1;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 踩点单双车道
    if (msg->pro_cmd == 1025)
    {
        if (msg->pro_ext2 == 1) // 双车道
        {
            std::cout << "***********************" << "启用踩点模式双车道" << "*****************************" << std::endl;
            modecaidian = 1;
        }
        if (msg->pro_ext2 == 0)
        {
            std::cout << "***********************" << "启用踩点模式单车道" << "*****************************" << std::endl;
            modecaidian = 0;
        }
    }
    // 解除雷达限制
    if (msg->pro_cmd == 1026)
    {
        if (msg->pro_ext2 == 1) //
        {
            auto start_time = std::chrono::system_clock::now();
            std::time_t start_time_t = std::chrono::system_clock::to_time_t(start_time);
            std::cout << "[" << std::ctime(&start_time_t) << "] "
                      << "*********************** 解除雷达限制 *****************************" << std::endl;
            Release_radar = 1;

            // 延迟重置线程
            std::thread reset_thread([]()
                                     {
                    // 记录开始等待的时间
                    auto wait_start = std::chrono::system_clock::now();
                    std::time_t wait_start_t = std::chrono::system_clock::to_time_t(wait_start);
                    std::cout << "[" << std::ctime(&wait_start_t) << "] "
                            << "开始等待3秒后恢复雷达限制..." << std::endl;

                    // 等待3秒
                    std::this_thread::sleep_for(std::chrono::seconds(3));

                    // 计算实际等待时间（毫秒）
                    auto wait_end = std::chrono::system_clock::now();
                    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(
                        wait_end - wait_start
                    ).count();

                    // 重置雷达限制
                    Release_radar = 0;
                    std::time_t wait_end_t = std::chrono::system_clock::to_time_t(wait_end);
                    std::cout << "[" << std::ctime(&wait_end_t) << "] "
                            << "*********************** " << duration << "毫秒后恢复雷达限制 *****************************" << std::endl; });

            reset_thread.detach();
        }
    }
    // 创建线路
    if (msg->pro_cmd == 12)
    {
        if (msg->pro_ext2 == 1 && !thread_instance_createline.joinable())
        {

            std::cout << "***********************" << "创建线路,地图编号:" << msg->pro_cmd_opt << "线路编号：" << msg->pro_ext1 << "*****************************" << std::endl;
            // 检查地图是否需要切换
            if (getmap_id != msg->pro_cmd_opt)
            {
                std::cout << "***********************" << "创建线路,getmap_id:" << getmap_id << ",msg->pro_cmd_opt" << msg->pro_cmd_opt << "*****************************" << std::endl;

                getmap_id = msg->pro_cmd_opt;
                map_ck(msg->pro_cmd_opt);
                sleep(3);
            }
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            sradsack_fast_data.path_id = msg->pro_ext1;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
            should_run.store(true);
            thread_instance_createline = std::thread(std::bind(thread_Create_line, msg->pro_cmd_opt, msg->pro_ext1, msg->pro_nema));
        }
        if (msg->pro_ext2 == 0 && thread_instance_createline.joinable())
        {
            should_run.store(false);
            if (thread_instance_createline.joinable())
            {
                thread_instance_createline.join(); // 等待线程结束
            }
            std::cout << "***********************" << "结束创建线路,地图编号:" << msg->pro_cmd_opt << "线路编号：" << msg->pro_ext1 << "*****************************" << std::endl;
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            sradsack_fast_data.path_id = msg->pro_ext1;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 创建线路B
    if (msg->pro_cmd == 1024)
    {
        if (msg->pro_ext2 == 1 && !thread_instance_createlineB.joinable())
        {

            std::cout << "***********************" << "创建线路B,地图编号:" << msg->pro_cmd_opt << "线路编号：" << msg->pro_ext1 << "*****************************" << std::endl;
            // 检查地图是否需要切换
            if (getmap_id != msg->pro_cmd_opt)
            {
                getmap_id = msg->pro_cmd_opt;
                map_ck(msg->pro_cmd_opt);
                sleep(3);
            }
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            sradsack_fast_data.path_id = msg->pro_ext1;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
            should_runB.store(true);
            thread_instance_createlineB = std::thread(std::bind(thread_Create_lineB, msg->pro_cmd_opt, msg->pro_ext1, msg->pro_nema));
        }
        if (msg->pro_ext2 == 0 && thread_instance_createlineB.joinable())
        {
            should_runB.store(false);
            if (thread_instance_createlineB.joinable())
            {
                thread_instance_createlineB.join(); // 等待线程结束
            }
            std::cout << "***********************" << "结束创建线路B,地图编号:" << msg->pro_cmd_opt << "线路编号：" << msg->pro_ext1 << "*****************************" << std::endl;
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            sradsack_fast_data.path_id = msg->pro_ext1;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 开始记忆线路
    if (msg->pro_cmd == 2)
    {
        if (msg->pro_cmd_opt == 0 && !thread_instance_createline_goback.joinable()) // 开始记忆
        {

            std::cout << "***********************" << "启动记忆" << "*****************************" << std::endl;
            char command[256];
            snprintf(command, sizeof(command), "cd /home/argo/catkin_ws/src/rads-master/shell && ./starttu.sh &");
            int result = system(command);
            sleep(3);
            should_run_goback.store(true);
            thread_instance_createline_goback = std::thread(std::bind(thread_Create_line_goback));
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
        if (msg->pro_cmd_opt == 1) // 清除记忆
        {

            std::cout << "***********************" << "清除记忆开始" << "*****************************" << std::endl;
            // 关闭返航线程
            should_run_goback_play.store(false);
            if (thread_instance_createline_goback_play.joinable())
            {
                thread_instance_createline_goback_play.join();
                std::cout << "***********************" << "关闭返航线程" << "*****************************" << std::endl;
            }
            // 关闭记忆
            should_run_goback.store(false);
            if (thread_instance_createline_goback.joinable())
            {
                thread_instance_createline_goback.join(); // 等待线程结束
                std::cout << "***********************" << "关闭记忆" << "*****************************" << std::endl;
            }
            // 清空数据库
            delete_go_back();
            std::cout << "***********************" << "清空数据库" << "*****************************" << std::endl;

            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }

    // 开始返航
    if (msg->pro_cmd == 3)
    {
        if (msg->pro_cmd_opt == 0) //   开始返航
        {
            std::cout << "***********************" << "开始返航" << "*****************************" << std::endl;
            // 首先关闭记忆
            should_run_goback.store(false);
            if (thread_instance_createline_goback.joinable())
            {
                thread_instance_createline_goback.join(); // 等待线程结束
                std::cout << "***********************" << "关闭记忆" << "*****************************" << std::endl;
            }
            // 关闭自动驾驶
            stopRadarThread.store(true);
            pubdrive(0, 2, 18000, 0);
            sleep(1);
            // 创建返航线程
            if (!thread_instance_createline_goback_play.joinable())
            {
                // 首次启动返航线程

                stop_car = 1;
                current_target_index = 0;
                goback_forward = true;  // 初始方向：正向
                need_turnaround = true; // true  需要掉头  false 不需要掉头
                should_run_goback_play.store(true);
                thread_instance_createline_goback_play = std::thread(std::bind(thread_Create_line_goback_play));
                std::cout << "***********************" << "创建返航线程" << "*****************************" << std::endl;
            }
            else
            {
                if (should_run_goback_play.load())
                {
                    goback_forward = !goback_forward; // 反向行驶
                    stop_car = 1;
                    need_turnaround = true; // true  需要掉头  false 不需要掉头
                    std::cout << "切换方向为: " << (goback_forward ? "正向" : "反向")
                              << ", 当前点: " << current_target_index.load() << std::endl;
                    std::cout << "***********************" << "返航掉头" << "*****************************" << std::endl;
                }
                else
                {
                    std::cout << "***********************" << "返航线程未启动" << "*****************************" << std::endl;
                }
            }
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }

        if (msg->pro_cmd_opt == 1) //   暂停||继续
        {
            stop_car = (stop_car == 1) ? 2 : 1;
            std::cout << "***********************" << "暂停||继续：" << stop_car << "************************" << std::endl;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 设置开机点
    if (msg->pro_cmd == 16)
    {
        if (msg->pro_ext2 == 1)
        {
            std::cout << "***********************" << "上传开机点，地图编号:" << msg->pro_cmd_opt << "开机点编号：" << msg->pro_ext1 << "*****************************" << std::endl;
            // 检查地图是否需要切换
            if (getmap_id != msg->pro_cmd_opt)
            {
                getmap_id = msg->pro_cmd_opt;
                map_ck(msg->pro_cmd_opt);
                sleep(3);
            }
            // 上报
            rads::rads_acks rads_acks_msg;
            rads_acks_msg.header.stamp = ros::Time::now();
            rads_acks_msg.pro_nema = msg->pro_nema;
            rads_acks_msg.cmd = msg->pro_cmd;
            rads_acks_msg.cmd_opt = msg->pro_cmd_opt;
            rads_acks_msg.ext1 = msg->pro_ext1;
            rads_acks_msg.ext2 = msg->pro_ext2;
            rads_acks_msg.ack = 1;
            rads_acks_msg.map_id = msg->pro_cmd_opt;
            nav_msgs::Path trajectory;
            geometry_msgs::PoseStamped pose;
            pose.pose.position.x = lidar_rtk.x;
            pose.pose.position.y = lidar_rtk.y;
            pose.pose.position.z = lidar_rtk.z;
            pose.pose.orientation.x = lidar_rtk.ox;
            pose.pose.orientation.y = lidar_rtk.oy;
            pose.pose.orientation.z = lidar_rtk.oz;
            pose.pose.orientation.w = lidar_rtk.ow;
            rads_acks_msg.trajectory.poses.push_back(pose);
            rads::rads_trackext trajectory_ext;
            trajectory_ext.point_count = msg->pro_ext1; // 开机点编号
            trajectory_ext.x = lidar_rtk.x;
            trajectory_ext.y = lidar_rtk.y;
            trajectory_ext.z = lidar_rtk.z;
            trajectory_ext.rtk_x = lidar_rtk.x;
            trajectory_ext.rtk_y = lidar_rtk.y;
            trajectory_ext.rtk_z = lidar_rtk.z;
            trajectory_ext.ld_heading = lidar_rtk.ld_heading;
            trajectory_ext.matching_rate = lidar_rtk.matching_rate;
            trajectory_ext.longitude = lidar_rtk.longitude;
            trajectory_ext.latitude = lidar_rtk.latitude;
            trajectory_ext.heading = lidar_rtk.heading;
            trajectory_ext.posi_flag = lidar_rtk.posi_flag;
            trajectory_ext.head_flag = lidar_rtk.head_flag;
            rads_acks_msg.trajectory_ext.push_back(trajectory_ext);
            sradsack_pub_.publish(rads_acks_msg);
        }
        if (msg->pro_ext2 == 0)
        {
            std::cout << "***********************" << "删除开机点，地图编号:" << msg->pro_cmd_opt << "开机点编号：" << msg->pro_ext1 << "*****************************" << std::endl;
            sradsack_fast_data.ack = 1;
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 创建地图
    if (msg->pro_cmd == 10)
    {
        if (msg->pro_ext2 == 1)
        {
            std::cout << "***********************" << "开始建图,地图编号:" << msg->pro_cmd_opt << "*****************************" << std::endl;
            char command[256];
            snprintf(command, sizeof(command), "cd /home/argo/catkin_ws/src/rads-master/shell && ./starttu.sh %d &", (int)msg->pro_cmd_opt);
            int result = system(command);
            sleep(10);
            sradsack_fast_data.ack = 1;
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
        if (msg->pro_ext2 == 0)
        {
            std::cout << "***********************" << "结束建图,地图编号:" << msg->pro_cmd_opt << "*****************************" << std::endl;
            char command[256];
            snprintf(command, sizeof(command), "cd /home/argo/catkin_ws/src/rads-master/shell && ./endtu.sh %d &", (int)msg->pro_cmd_opt);
            int result = system(command);
            sleep(25);
            std::string map_path = "/home/argo/catkin_ws/pcd/" + std::to_string(msg->pro_cmd_opt) + ".pcd";
            sradsack_fast_data.ack = 1;
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            sradsack_fast_data.map_path = map_path;
            publishacks(sradsack_pub_, sradsack_fast_data);
            getmap_id = msg->pro_cmd_opt;
            map_ck(msg->pro_cmd_opt);
        }
    }
    // 上装功能，摄像头功能为41，抓拍:摄像头机位3,本次抓拍识别码32
    if (msg->pro_cmd == 4 && msg->pro_cmd_opt == 1)
    {

        std::cout << "***********************" << "抓拍:摄像头机位:" << msg->pro_ext1 << "本次抓拍识别码：" << msg->pro_ext2 << "*****************************" << std::endl;
        int ck = Capture(msg->pro_ext2);
        if (ck == 0)
        {
            sradsack_fast_data.ack = 1;
            std::string img_path = "/home/argo/catkin_ws/src/rads-master/img/" + std::to_string(msg->pro_ext2) + ".jpeg";
            sradsack_fast_data.map_path = img_path;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
        else
        {
            sradsack_fast_data.ack = 2;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
}
// 接受融合后的雷达+RTK数据
void laserCallback(const rads::rads_trackext::ConstPtr &msg)
{
    // 获取雷达数据
    lidar_rtk.x = isnan(msg->x) ? 0.0 : msg->x;
    lidar_rtk.y = isnan(msg->y) ? 0.0 : msg->y;
    lidar_rtk.z = isnan(msg->z) ? 0.0 : msg->z;
    lidar_rtk.ox = isnan(msg->ox) ? 0.0 : msg->ox;
    lidar_rtk.oy = isnan(msg->oy) ? 0.0 : msg->oy;
    lidar_rtk.oz = isnan(msg->oz) ? 0.0 : msg->oz;
    lidar_rtk.ow = isnan(msg->ow) ? 0.0 : msg->ow;
    lidar_rtk.matching_rate = isnan(msg->matching_rate) ? 0.0 : msg->matching_rate;
    // 处理四元数数据
    lidar_rtk.ld_heading = isnan(quaternionToEulerAngle(msg->ox, msg->oy, msg->oz, msg->ow)) ? 0.0 : quaternionToEulerAngle(msg->ox, msg->oy, msg->oz, msg->ow);
    // 获取RTK数据
    double x1 = isnan(msg->rtk_x) ? 0.0 : msg->rtk_x;
    double y1 = isnan(msg->rtk_y) ? 0.0 : msg->rtk_y;
    double yaw = isnan(msg->heading) ? 0.0 : msg->heading;
    double baseline = RTK_BASELINE;
    double xMid, yMid;
    calculateMidPoint(x1, y1, yaw, baseline, xMid, yMid);
    lidar_rtk.rtk_x = isnan(yMid) ? 0.0 : yMid;
    lidar_rtk.rtk_y = isnan(xMid) ? 0.0 : xMid;
    lidar_rtk.rtk_z = isnan(msg->rtk_z) ? 0.0 : msg->rtk_z;
    lidar_rtk.heading = isnan(msg->heading) ? 0.0 : msg->heading;
    lidar_rtk.longitude = isnan(msg->longitude) ? 0.0 : msg->longitude;
    lidar_rtk.latitude = isnan(msg->latitude) ? 0.0 : msg->latitude;
    lidar_rtk.posi_flag = isnan(msg->posi_flag) ? 0.0 : msg->posi_flag;
    lidar_rtk.head_flag = isnan(msg->head_flag) ? 0.0 : msg->head_flag;
    lidar_rtk.groud_speed = isnan(msg->groud_speed) ? 0.0 : msg->groud_speed;
    lidar_rtk.base_length = isnan(msg->flag) ? 0.0 : msg->flag;

    // lidar_rtk.matching_rate=90.0;
    // lidar_rtk.x=-74.988266;
    // lidar_rtk.y=79.888809;
    // lidar_rtk.ld_heading=89.733643;
}
// 避障（停）
void points_rawCallback(const sensor_msgs::PointCloud2ConstPtr &msg_pc)
{
    pcl::PointCloud<pcl::PointXYZ> pc;
    pcl::fromROSMsg(*msg_pc, pc);
    int Stop_3Dpoint = 0, BStop_3Dpoint = 0, BStop_3Dpoint_1 = 0, BStop_3Dpoint_2 = 0, BStop_3Dpoint_3 = 0, BStop_3Dpoint_4 = 0, BStop_3Dpoint_5 = 0, BStop_3Dpoint_6 = 0, BStop_3Dpoint_7 = 0;
    int lj_3Dpoint_1 = 500, lj_3Dpoint_2 = 1000, lj_3Dpoint_3 = 100, lj_3Dpoint_4 = 100, lj_3Dpoint_5 = 100, lj_3Dpoint_6 = 500, lj_3Dpoint_7 = 1000;
    for (const pcl::PointXYZ &p : pc.points)
    {
        if (p.x <= MAX_X && p.x >= MIN_X && p.z >= MIN_Z && p.y >= MIN_Y && p.y <= MAX_Y) // 停障区(p.x)/激光雷达左右宽度,(p.y);//激光雷达前后(p.z);//激光雷达高度差
        {
            Stop_3Dpoint++;
        }
        if (p.x <= MAX_X && p.x >= MIN_X && p.z >= MIN_Z && p.y >= BMIN_Y && p.y <= BMAX_Y) // 0
        {
            BStop_3Dpoint++;
        }
        if (p.x <= 3.0 && p.x >= 0.75 && p.y >= 0.1 && p.y <= 2.63 && p.z >= -0.7 && p.z <= 1.0) // 1
        {
            BStop_3Dpoint_1++;
        }
        if (p.x <= 3.0 && p.x >= 0.75 && p.y >= -1.9 && p.y <= 0.0 && p.z >= -0.7 && p.z <= 1.0) // 2区域
        {
            BStop_3Dpoint_2++;
        }
        if (p.x <= 3.0 && p.x >= 0.75 && p.y >= -4.53 && p.y <= -1.9 && p.z >= -0.7 && p.z <= 1.0) // 3区域
        {
            BStop_3Dpoint_3++;
        }
        if (p.x <= 0.75 && p.x >= -0.75 && p.y >= -4.53 && p.y <= -1.9 && p.z >= -0.7 && p.z <= 1.0) // 4区域
        {
            BStop_3Dpoint_4++;
        }
        if (p.x <= -0.75 && p.x >= -3.0 && p.y >= -4.53 && p.y <= -1.9 && p.z >= -0.7 && p.z <= 1.0) // 5区域
        {
            BStop_3Dpoint_5++;
        }
        if (p.x <= -0.75 && p.x >= -3.0 && p.y >= -1.9 && p.y <= 0.0 && p.z >= -0.7 && p.z <= 1.0) // 6区域
        {
            BStop_3Dpoint_6++;
        }
        if (p.x <= -0.75 && p.x >= -3.0 && p.y >= 0.1 && p.y <= 2.63 && p.z >= -0.7 && p.z <= 1.0) // 7区域
        {
            BStop_3Dpoint_7++;
        }
    }
    if (RADAR_TEST == 1)
    {
        std::cout << "停障点数:" << Stop_3Dpoint << std::endl;
        std::cout << "区域0点数:" << BStop_3Dpoint << std::endl;
        std::cout << "区域1点数:" << BStop_3Dpoint_1 << std::endl;
        std::cout << "区域2点数:" << BStop_3Dpoint_2 << std::endl;
        std::cout << "区域3点数:" << BStop_3Dpoint_3 << std::endl;
        std::cout << "区域4点数:" << BStop_3Dpoint_4 << std::endl;
        std::cout << "区域5点数:" << BStop_3Dpoint_5 << std::endl;
        std::cout << "区域6点数:" << BStop_3Dpoint_6 << std::endl;
        std::cout << "区域7点数:" << BStop_3Dpoint_7 << std::endl;
    }
    ros::param::get("RADAR_F", RADAR_F);
    if (Stop_3Dpoint > STOP_3D_PONIT && RADAR_F == 1)
    {
        Stop_lidar = 1; // 遇到障碍停车
    }
    else
    {
        Stop_lidar = 0; // 没有障碍正常跑
    }
    // 变道
    if (BStop_3Dpoint > BSTOP_3D_PONIT && RADAR_F == 1)
    {
        Stop_lidar0 = 1;
    }
    else
    {
        Stop_lidar0 = 0;
    }

    if ((BStop_3Dpoint_1 - lj_3Dpoint_1) > BSTOP_3D_PONIT && Release_radar == 0)
    {
        Stop_lidar1 = 1;
    }
    else
    {
        Stop_lidar1 = 0;
    }

    if ((BStop_3Dpoint_2 - lj_3Dpoint_2) > BSTOP_3D_PONIT && Release_radar == 0)
    {
        Stop_lidar2 = 1;
    }
    else
    {
        Stop_lidar2 = 0;
    }

    if ((BStop_3Dpoint_3 - lj_3Dpoint_3) > BSTOP_3D_PONIT && Release_radar == 0)
    {
        Stop_lidar3 = 1;
    }
    else
    {
        Stop_lidar3 = 0;
    }

    if ((BStop_3Dpoint_4 - lj_3Dpoint_4) > BSTOP_3D_PONIT)
    {
        Stop_lidar4 = 1;
    }
    else
    {
        Stop_lidar4 = 0;
    }

    if ((BStop_3Dpoint_5 - lj_3Dpoint_5) > BSTOP_3D_PONIT && Release_radar == 0)
    {
        Stop_lidar5 = 1;
    }
    else
    {
        Stop_lidar5 = 0;
    }

    if ((BStop_3Dpoint_6 - lj_3Dpoint_6) > BSTOP_3D_PONIT && Release_radar == 0)
    {
        Stop_lidar6 = 1;
    }
    else
    {
        Stop_lidar6 = 0;
    }

    if ((BStop_3Dpoint_7 - lj_3Dpoint_7) > BSTOP_3D_PONIT && Release_radar == 0)
    {
        Stop_lidar7 = 1;
    }
    else
    {
        Stop_lidar7 = 0;
    }
}
// 下位机来的数据
void pradsscmdCallback(const rads::rads_scmd::ConstPtr &msg)
{
    int model = msg->model; // （1:遥控，2:跟随和推，3:返航，4:无人驾驶）
    if (model != 4)         // 模式不等于4
    {
        stopRadarThread.store(true);   // 关闭自动驾驶
        stopRadarThread_R.store(true); // 关闭自动驾驶
        should_run_goback_play.store(false); // 关闭返航
    }
    pcbmsg.model = isnan(msg->model) ? 0.0 : msg->model;
    pcbmsg.soc = isnan(msg->soc) ? 0.0 : msg->soc;
    pcbmsg.err = isnan(msg->err) ? 0.0 : msg->err;
    pcbmsg.rpm_l = isnan(msg->rpm_l) ? 0.0 : msg->rpm_l;
    pcbmsg.rpm_r = isnan(msg->rpm_r) ? 0.0 : msg->rpm_r;
    pcbmsg.key_1 = isnan(msg->key_1) ? 0.0 : msg->key_1;
    pcbmsg.key_2 = isnan(msg->key_2) ? 0.0 : msg->key_2;
    pcbmsg.key_3 = isnan(msg->key_3) ? 0.0 : msg->key_3;
    pcbmsg.key_4 = isnan(msg->key_4) ? 0.0 : msg->key_4;
    pcbmsg.key_5 = isnan(msg->key_5) ? 0.0 : msg->key_5;
    pcbmsg.key_6 = isnan(msg->key_6) ? 0.0 : msg->key_6;
    pcbmsg.key_7 = isnan(msg->key_7) ? 0.0 : msg->key_7;
    pcbmsg.key_8 = isnan(msg->key_8) ? 0.0 : msg->key_8;
    pcbmsg.ladar_1 = isnan(msg->ladar_1) ? 0.0 : msg->ladar_1;
    pcbmsg.ladar_2 = isnan(msg->ladar_2) ? 0.0 : msg->ladar_2;
    pcbmsg.ladar_3 = isnan(msg->ladar_3) ? 0.0 : msg->ladar_3;
    pcbmsg.ladar_4 = isnan(msg->ladar_4) ? 0.0 : msg->ladar_4;
    pcbmsg.ladar_5 = isnan(msg->ladar_5) ? 0.0 : msg->ladar_5;
    pcbmsg.ladar_6 = isnan(msg->ladar_6) ? 0.0 : msg->ladar_6;
    pcbmsg.ladar_7 = isnan(msg->ladar_7) ? 0.0 : msg->ladar_7;
    pcbmsg.ladar_8 = isnan(msg->ladar_8) ? 0.0 : msg->ladar_8;
    pcbmsg.unknow_1 = isnan(msg->unknow_1) ? 0.0 : msg->unknow_1;
    pcbmsg.unknow_2 = isnan(msg->unknow_2) ? 0.0 : msg->unknow_2;
    pcbmsg.unknow_3 = isnan(msg->unknow_3) ? 0.0 : msg->unknow_3;
    pcbmsg.unknow_4 = isnan(msg->unknow_4) ? 0.0 : msg->unknow_4;
    pcbmsg.unknow_5 = isnan(msg->unknow_5) ? 0.0 : msg->unknow_5;
    pcbmsg.unknow_6 = isnan(msg->unknow_6) ? 0.0 : msg->unknow_6;
}
// 双光谱摄像头报警
void pradscameraCallback(const rads::rads_camera::ConstPtr &msg)
{
    ros::param::get("KTV_SN", KTV_SN);
    int device_id = std::stoi(KTV_SN);

    // 获取当前位置
    double current_latitude = lidar_rtk.latitude;
    double current_longitude = lidar_rtk.longitude;

    // 获取当前时间
    ros::Time current_time = ros::Time::now();

    // 检查是否需要发布报警
    bool should_publish = false;

    // 首次报警或满足距离/时间条件
    if (is_first_alarm)
    {
        should_publish = true;
        is_first_alarm = false;
    }
    else
    {
        // 计算距离（米）
        double distance = calculateDistance(last_latitude, last_longitude, current_latitude, current_longitude);

        // 计算时间差（秒）
        ros::Duration time_diff = current_time - last_alarm_time;

        // 判断是否满足条件：距离大于5米或时间超过3分钟
        should_publish = (distance > 5.0) || (time_diff.toSec() > 10.0);
    }

    if (should_publish)
    {
        // 更新最后报警位置和时间
        last_latitude = current_latitude;
        last_longitude = current_longitude;
        last_alarm_time = current_time;

        // 执行拍照
        int ck = Capture(device_id);

        if (ck == 0)
        {
            // 复制消息内容
            rads::rads_camera msginfo;
            msginfo.device_id = std::to_string(device_id);
            msginfo.devIp = msg->devIp;
            msginfo.Alarmtype = msg->Alarmtype;
            msginfo.preset = msg->preset;
            msginfo.preAlarmSum = msg->preAlarmSum;
            msginfo.alarmSum = msg->alarmSum;
            msginfo.mtAlarmInfo = msg->mtAlarmInfo;
            msginfo.file = "/home/argo/catkin_ws/src/rads-master/img/" + std::to_string(device_id) + ".jpeg";
            msginfo.thermalfile = "/home/argo/catkin_ws/src/rads-master/reimg/" + std::to_string(device_id) + ".jpeg";

            // 发布报警消息
            pradscamerapub.publish(msginfo);

            // 使用ROS时间格式化函数替代toString()
            // char time_str[100];
            // sprintf(time_str, "%u.%09u", current_time.sec, current_time.nsec);
            // ROS_INFO("Alarm published at position: lat=%.6f, lon=%.6f, time=%s",
            //         current_latitude, current_longitude, time_str);

            ck = -1;
        }
    }
    else
    {
        // 使用ROS时间格式化函数替代toString()
        // char time_str[100];
        // sprintf(time_str, "%u.%09u", current_time.sec, current_time.nsec);
        // ROS_INFO("Alarm suppressed: distance=%.2f m, time_diff=%.1f s, current_time=%s",
        //         calculateDistance(last_latitude, last_longitude, current_latitude, current_longitude),
        //         (current_time - last_alarm_time).toSec(), time_str);
    }
}

// 接受goback定位
void odometryCallgoback(const nav_msgs::Odometry::ConstPtr &msg)
{

    lidar_gobackmsg.x = isnan(msg->pose.pose.position.x) ? 0.0 : msg->pose.pose.position.x;
    lidar_gobackmsg.y = isnan(msg->pose.pose.position.y) ? 0.0 : msg->pose.pose.position.y;
    lidar_gobackmsg.z = isnan(msg->pose.pose.position.z) ? 0.0 : msg->pose.pose.position.z;
    lidar_gobackmsg.ld_heading = isnan(quaternionToEulerAngle(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w)) ? 0.0 : quaternionToEulerAngle(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
}

// 接受imu  sensor_msgs::Imu
void callbackimu(const sensor_msgs::Imu &msg)
{
    imu_msg.x = msg.orientation.x;
    imu_msg.y = msg.orientation.y;
    imu_msg.z = msg.orientation.z;
}

// 声明一个辅助函数来发布任务状态消息，减少重复代码
void publishTaskStatus(int cron_id, int task_num, int status)
{

            std::cout << "***********************" << "44555666777" << "*****************************" << std::endl;

    car_cron task = getcron(cron_id);
    rads::rads_cronapi msg;

    msg.header.stamp = ros::Time::now();
    msg.ts = task.ts;
    msg.uid = task.uid;
    msg.vehicleCode = task.vehicleCode;
    msg.executeType = task.executeType;
    msg.taskCode = task.taskCode;
    msg.num = task_num;
    msg.status = status; // 1 开始，2结束，3失败
    msg.mapid=getmap_id;
    msg.turn= task.turn;
    msg.uptime=task.uptime;
    pcronapi.publish(msg);

    // 根据状态输出不同日志
    const char *status_str[] = {"", "开始", "结束", "失败"};
    if (task_num == 0)
    {
        std::cout << "总任务包" << status_str[status] << "执行,总任务编号: " << task.taskCode << std::endl;
    }
    else
    {
        std::cout << "子任务包" << status_str[status] << "执行,任务编号: " << task_num << std::endl;
    }
}

void publishCurrentTask(int current_task_index)
{
    if (cron_id == 0)
        return; // 增加对无效cron_id的检查

    auto task_list = fetchCarcrondata(cron_id);
    if (current_task_index >= 0 && current_task_index < task_list.size())
    {
        auto &task = task_list[current_task_index];
        // 帮我写下  检测 ask.data.c_str()  是否包含  2个##   如果是2个##  需要合并后的指令
        // 这边收到的到就是  ask.data = ##1500001,29,1247,244,3,1,5,1,##1500001,291,1247,244,3,1,1,1  需要  合并成 ##1500001,299,1247,244,3,1,5,1,1,1,1
        std::string result1 = mergeInstructions(task.data.c_str());
        // 构造并发布指令
        NetCmdObsolete cmd = DecodeNetCmdObsolete(result1);
        rads::rads_cmd _cmd;
        Cmd2radsmsg(cmd, _cmd);
        p.publish(_cmd);

        // 发布任务开始状态
        publishTaskStatus(cron_id, task.num, 1);

        ROS_WARN("[Task %d/%zu] Sending: %s",
                 task.num,
                 task_list.size(),
                 task.data.c_str());
    }
    else
    {
        ROS_INFO("All tasks completed!");
        publishTaskStatus(cron_id, 0, 2); // 总任务结束
        cron_id = 0;                      // 总任务结束，总任务ID置零
    }
}

void car_cronCallback(const std_msgs::String::ConstPtr &msg)
{
    ROS_INFO("Received message: %s", msg->data.c_str());

    try
    {
        cron_id = std::stoi(msg->data);
    }
    catch (...)
    {
        ROS_ERROR("Invalid cron_id received: %s", msg->data.c_str());
        return;
    }

    // 发送总任务开始消息
    carcron_add(cron_id);
    publishTaskStatus(cron_id, 0, 1);
    
    // 获取任务列表并开始执行
    current_task_index = 0; // 重置索引
    publishCurrentTask(current_task_index);
}

void cronAckCallback(const rads::rads_cron::ConstPtr &msg)
{
    if (cron_id == 0)
        return; // 无效cron_id直接返回

    auto task_list = fetchCarcrondata(cron_id);
    if (current_task_index < 0 || current_task_index >= task_list.size())
    {
        return; // 无效索引直接返回
    }

    auto &task = task_list[current_task_index];

    switch (msg->ack)
    {
    case 1: // 成功完成
        publishTaskStatus(cron_id, task.num, 2);
        break;
    case 2: // 执行失败
        publishTaskStatus(cron_id, task.num, 3);
        break;
    default:
        return; // 无效ack值直接返回
    }

    current_task_index++; // 移动到下一个任务
    publishCurrentTask(current_task_index);
}

void publishThread()
{
    ros::Rate rate(5); // 1Hz发布频率
    double speed_kmh = lidar_rtk.groud_speed * 3.6;
    while (ros::ok())
    {
        std::stringstream ss;
        ss << "{"
           << "\"id\":" << KTV_SN << ","
           << "\"speed\":" << speed_kmh << ","
           << "\"battery\":" << pcbmsg.soc << ","
           << "\"mode\":" << pcbmsg.model
           << "}";

        std_msgs::String msg;
        msg.data = ss.str();
        papp.publish(msg);
        // ROS_INFO("Published: %s", msg.data.c_str());
        rate.sleep();
    }
}

void vehicleCommandCallback(const std_msgs::String::ConstPtr &msg)
{
    rapidjson::Document document;
    if (document.Parse(msg->data.c_str()).HasParseError())
    {
        std::cout << "解析 /vehicle_command 消息时出错" << std::endl;
        return;
    }

    if (document.HasMember("command") && document["command"].IsString())
    {
        std::cout << "接收到指令command:" << document["command"].GetString() << std::endl;
    }
    else
    {
        std::cout << "/vehicle_command 消息格式不正确" << std::endl;
    }
}

void vehicleControlCallback(const std_msgs::String::ConstPtr &msg)
{
    rapidjson::Document document;
    if (document.Parse(msg->data.c_str()).HasParseError())
    {
        std::cout << "解析 /vehicle_control 消息时出错" << std::endl;

        return;
    }

    if (document.HasMember("data") && document["data"].IsObject())
    {
        const rapidjson::Value &data = document["data"];
        if (data.HasMember("speed") && data["speed"].IsNumber() &&
            data.HasMember("steering") && data["steering"].IsNumber())
        {
            double speed = data["speed"].GetDouble();
            double steering = data["steering"].GetDouble();
            std::cout << "接收到控制数据- 左:" << speed << ",右:" << steering << std::endl;
        }
        else
        {
            std::cout << "/vehicle_control 消息中的data格式不正确" << std::endl;
        }
    }
    else
    {
        std::cout << "/vehicle_control 消息格式不正确" << std::endl;
    }
}

int main(int argc, char **argv)
{

    stopRadarThread.store(true);         // 初始化关闭自动驾驶
    stopRadarThread_R.store(true);       // 初始化关闭自动驾驶
    should_run_goback_play.store(false); // 初始化关闭返航
    ros::init(argc, argv, NODE_BUSINESS);
    ros::NodeHandle nh_;
    ros::param::get("AUTO_DISRANCE", AUTO_DISRANCE);
    ros::param::get("HEADING_THRESHOLD", HEADING_THRESHOLD);
    ros::param::get("DRIVE_DISRANCE", DRIVE_DISRANCE);
    ros::param::get("DRIVE_DISRANCE_LAST", DRIVE_DISRANCE_LAST);
    ros::param::get("MAXSPEED", MAXSPEED);
    ros::param::get("MINSPEED", MINSPEED);
    ros::param::get("RADS_B", RADS_B);
    ros::param::get("RADS_R", RADS_R);
    ros::param::get("MAX_X", MAX_X);
    ros::param::get("MIN_X", MIN_X);
    ros::param::get("MAX_Y", MAX_Y);
    ros::param::get("MIN_Y", MIN_Y);
    ros::param::get("BMAX_Y", BMAX_Y);
    ros::param::get("BMIN_Y", BMIN_Y);
    ros::param::get("MIN_Z", MIN_Z);
    ros::param::get("STOP_3D_PONIT", STOP_3D_PONIT);
    ros::param::get("BSTOP_3D_PONIT", BSTOP_3D_PONIT);
    ros::param::get("RADAR_TEST", RADAR_TEST);
    ros::param::get("HEADING_ERROR", HEADING_ERROR);
    ros::param::get("RTK_BASELINE", RTK_BASELINE);
    ros::param::get("PID_STATUS", PID_STATUS);
    ros::param::get("PID_STATUS", CONFIG_KP);
    ros::param::get("PID_STATUS", CONFIG_KI);
    ros::param::get("PID_STATUS", CONFIG_KD);
    ros::param::get("PID_STATUS", CONFIG_MIN_OUTPUT);
    ros::param::get("PID_STATUS", CONFIG_MAX_OUTPUT);
    ros::param::get("KTV_SN", KTV_SN);

    getmap_id = getmapid();
    ROS_INFO("getmap_id: %d", getmap_id);
    map_ck(getmap_id);
    // Demo_Capture();
    //  订阅网络发来指令
    static ros::Subscriber sub_ = nh_.subscribe("pradscmd", 1000, pradscmdCallback);
    // 订阅融合后的雷达+RTK数据
    static ros::Subscriber ndt_pose = nh_.subscribe("pradsfusion", 1, laserCallback); // ndt_pose
    // 订阅points_raw 用于激光雷达停障
    // static ros::Subscriber sub_points_raw = nh_.subscribe<sensor_msgs::PointCloud2>("/points_raw", 100, points_rawCallback);
    static ros::Subscriber sub_points_raw = nh_.subscribe<sensor_msgs::PointCloud2>("/rslidar_points", 100, points_rawCallback);
    // 订阅下位机发来的指令 pradsscmd
    static ros::Subscriber sub_pradsscmd = nh_.subscribe("pradsscmd", 1, pradsscmdCallback);
    // 订阅返航定位
    static ros::Subscriber sub_pradsscmd_goback_weizhi = nh_.subscribe("/odometry/imu", 1, odometryCallgoback);
    // 订阅双光普摄像头报警信息
    static ros::Subscriber sub_pradscamera = nh_.subscribe("pradscamera", 1, pradscameraCallback);
    // sensor_msgs::Imu
    static ros::Subscriber imusub = nh_.subscribe("imu603", 1, callbackimu);
    // 发布响应网络信息（主要用于响应网络指令，上传地图，上传路线点等）
    sradsack_pub_ = nh_.advertise<rads::rads_acks>("sradsack", 1000);
    // 发布控制车辆的信息
    sradsdrive = nh_.advertise<rads::rads_scmdret>("sradsdriveackermann", 10);
    // 发布地图重定位
    initialposepub = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1);
    // 发布双光普摄像头报警信息给服务器
    pradscamerapub = nh_.advertise<rads::rads_camera>("pradscamerainfo", 10);
    // 订阅cron话题
    static ros::Subscriber cronsub = nh_.subscribe("/my_car_cron", 10, car_cronCallback);
    // 发布来自网络的简要指令集
    p = nh_.advertise<rads::rads_cmd>("/pradscmd", QUEUE_SIZE);
    // 发布任务完成后话题
    pcron = nh_.advertise<rads::rads_cron>("/radsscron", QUEUE_SIZE);
    // 发布任务完成后话题
    pcronapi = nh_.advertise<rads::rads_cronapi>("/radsscronapi", QUEUE_SIZE);
    Rradar = nh_.advertise<std_msgs::String>("/ros_radar", QUEUE_SIZE);
    // 发布app话题
    papp = nh_.advertise<std_msgs::String>("/ros_to_mqtt", QUEUE_SIZE);
    // 订阅cron话题
    static ros::Subscriber radscronsub = nh_.subscribe("/radsscron", 10, cronAckCallback);
    // 订阅APP按钮话题
    ros::Subscriber command_sub = nh_.subscribe("/vehicle_command", 10, vehicleCommandCallback);
    // 订阅APP遥控
    ros::Subscriber control_sub = nh_.subscribe("/vehicle_control", 10, vehicleControlCallback);

    std::thread pub_thread(publishThread);
    ros::spin();
    pub_thread.join();
    return 0;
}
